From 68ab69de010adbe37b37558824ca013ecd0d569a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:47:51 +0200 Subject: moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results --- src/modules/commander/commander.cpp | 1720 +++++++++++++++++++++++++++++++++++ 1 file changed, 1720 insertions(+) create mode 100644 src/modules/commander/commander.cpp (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp new file mode 100644 index 000000000..253b6295f --- /dev/null +++ b/src/modules/commander/commander.cpp @@ -0,0 +1,1720 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Petri Tanskanen + * Lorenz Meier + * Thomas Gubler + * Julian Oes + * + * 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 commander.cpp + * Main system state machine implementation. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "commander_helper.h" +#include "state_machine_helper.h" +#include "calibration_routines.h" +#include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +extern struct system_load_s system_load; + +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + +/* Decouple update interval and hysteris counters, all depends on intervals */ +#define COMMANDER_MONITORING_INTERVAL 50000 +#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) +#define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define STICK_ON_OFF_LIMIT 0.75f +#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 +#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define GPS_FIX_TYPE_2D 2 +#define GPS_FIX_TYPE_3D 3 +#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 +#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) + +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + +/* Mavlink file descriptors */ +static int mavlink_fd; + +/* flags */ +static bool commander_initialized = false; +static bool thread_should_exit = false; /**< daemon exit flag */ +static bool thread_running = false; /**< daemon status flag */ +static int daemon_task; /**< Handle of daemon task / thread */ + +/* timout until lowlevel failsafe */ +static unsigned int failsafe_lowlevel_timeout_ms; + +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + + +/** + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + * + * @ingroup apps + */ +extern "C" __EXPORT int commander_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +void usage(const char *reason); + +/** + * React to commands that are sent e.g. from the mavlink module. + */ +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); + +/** + * Mainloop of commander. + */ +int commander_thread_main(int argc, char *argv[]); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); + + +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} + +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +{ + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + + /* request to set different system mode */ + switch (cmd->command) { + case VEHICLE_CMD_DO_SET_MODE: + + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + + if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { + /* reboot is executed later, after positive tune is sent */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); + } + } + } + break; + + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: + + if (((int)(cmd->param1)) == 0) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + + } else if (((int)(cmd->param1)) == 1) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + break; + + default: + mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + break; + } + + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_FAILED || + result == VEHICLE_CMD_RESULT_DENIED || + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + + tune_negative(); + + } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + + tune_positive(); + } + + /* send any requested ACKs */ + if (cmd->confirmation > 0) { + /* send acknowledge command */ + // XXX TODO + } + +} + +int commander_thread_main(int argc, char *argv[]) +{ + /* not yet initialized */ + commander_initialized = false; + bool home_position_set = false; + + bool battery_tune_played = false; + bool arm_tune_played = false; + + /* set parameters */ + failsafe_lowlevel_timeout_ms = 0; + param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); + + param_t _param_sys_type = param_find("MAV_TYPE"); + param_t _param_system_id = param_find("MAV_SYS_ID"); + param_t _param_component_id = param_find("MAV_COMP_ID"); + + /* welcome user */ + warnx("[commander] starting"); + + /* pthread for slow low prio thread */ + pthread_t commander_low_prio_thread; + + /* initialize */ + if (led_init() != 0) { + warnx("ERROR: Failed to initialize leds"); + } + + if (buzzer_init() != OK) { + warnx("ERROR: Failed to initialize buzzer"); + } + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + } + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; + /* make sure we are in preflight state */ + memset(¤t_status, 0, sizeof(current_status)); + + /* armed topic */ + struct actuator_armed_s armed; + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; + + /* neither manual nor offboard control commands have been received */ + current_status.offboard_control_signal_found_once = false; + current_status.rc_signal_found_once = false; + + /* mark all signals lost as long as they haven't been found */ + current_status.rc_signal_lost = true; + current_status.offboard_control_signal_lost = true; + + /* allow manual override initially */ + control_mode.flag_external_manual_override_ok = true; + + /* flag position info as bad, do not allow auto mode */ + // current_status.flag_vector_flight_mode_ok = false; + + /* set battery warning flag */ + current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + + /* set safety device detection flag */ + /* XXX do we need this? */ + //current_status.flag_safety_present = false; + + // XXX for now just set sensors as initialized + current_status.condition_system_sensors_initialized = true; + + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + + /* advertise to ORB */ + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + /* publish current state machine */ + + /* publish the new state */ + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + + + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + + /* home position */ + orb_advert_t home_pub = -1; + struct home_position_s home; + memset(&home, 0, sizeof(home)); + + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } + + // XXX needed? + mavlink_log_info(mavlink_fd, "system is running"); + + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + + /* Start monitoring loop */ + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; + + /* To remember when last notification was sent */ + uint64_t last_print_time = 0; + + float voltage_previous = 0.0f; + + bool low_battery_voltage_actions_done; + bool critical_battery_voltage_actions_done; + + uint64_t last_idle_time = 0; + + uint64_t start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + bool new_data = false; + + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + + /* Subscribe to manual control data */ + int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp_man; + memset(&sp_man, 0, sizeof(sp_man)); + + /* Subscribe to offboard control data */ + int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s sp_offboard; + memset(&sp_offboard, 0, sizeof(sp_offboard)); + + /* Subscribe to global position */ + int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + struct vehicle_global_position_s global_position; + memset(&global_position, 0, sizeof(global_position)); + uint64_t last_global_position_time = 0; + + /* Subscribe to local position data */ + int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + struct vehicle_local_position_s local_position; + memset(&local_position, 0, sizeof(local_position)); + uint64_t last_local_position_time = 0; + + /* + * The home position is set based on GPS only, to prevent a dependency between + * position estimator and commander. RAW GPS is more than good enough for a + * non-flying vehicle. + */ + + /* Subscribe to GPS topic */ + int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + struct vehicle_gps_position_s gps_position; + memset(&gps_position, 0, sizeof(gps_position)); + + /* Subscribe to sensor topic */ + int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s sensors; + memset(&sensors, 0, sizeof(sensors)); + + /* Subscribe to differential pressure topic */ + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + memset(&diff_pres, 0, sizeof(diff_pres)); + uint64_t last_diff_pres_time = 0; + + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* Subscribe to parameters changed topic */ + int param_changed_sub = orb_subscribe(ORB_ID(parameter_update)); + struct parameter_update_s param_changed; + memset(¶m_changed, 0, sizeof(param_changed)); + + /* Subscribe to battery topic */ + int battery_sub = orb_subscribe(ORB_ID(battery_status)); + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + battery.voltage_v = 0.0f; + + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + + /* now initialized */ + commander_initialized = true; + thread_running = true; + + start_time = hrt_absolute_time(); + + while (!thread_should_exit) { + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + /* update parameters */ + if (!armed.armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed getting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + control_mode.flag_external_manual_override_ok = false; + + } else { + control_mode.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + + orb_check(sp_man_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); + } + + orb_check(sp_offboard_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + } + + orb_check(sensor_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + } + + orb_check(diff_pres_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + last_diff_pres_time = diff_pres.timestamp; + } + + orb_check(cmd_sub, &new_data); + + if (new_data) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + } + + /* update safety topic */ + orb_check(safety_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(safety), safety_sub, &safety); + } + + /* update global position estimate */ + orb_check(global_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); + last_global_position_time = global_position.timestamp; + } + + /* update local position estimate */ + orb_check(local_position_sub, &new_data); + + if (new_data) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + last_local_position_time = local_position.timestamp; + } + + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + + /* update battery status */ + orb_check(battery_sub, &new_data); + if (new_data) { + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + current_status.battery_voltage = battery.voltage_v; + current_status.condition_battery_voltage_valid = true; + + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + + } + + if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { + current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + } else { + current_status.battery_voltage = 0.0f; + } + + /* update subsystem */ + orb_check(subsys_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + current_status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + current_status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + current_status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_health &= ~info.subsystem_type; + } + } + + /* Slow but important 8 Hz checks */ + if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { + + /* XXX if armed */ + if (armed.armed) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { + /* ready to arm */ + led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not ready to arm, something is wrong */ + led_toggle(LED_AMBER); + } + + if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { + + /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ + if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + /* GPS lock */ + led_on(LED_BLUE); + + } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* no GPS lock, but GPS module is aquiring lock */ + led_toggle(LED_BLUE); + } + + } else { + /* no GPS info, don't light the blue led */ + led_off(LED_BLUE); + } + + + // /* toggle GPS led at 5 Hz in HIL mode */ + // if (current_status.flag_hil_enabled) { + // /* hil enabled */ + // led_toggle(LED_BLUE); + + // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + // /* toggle arming (red) at 5 Hz on low battery or error */ + // led_toggle(LED_AMBER); + // } + + } + + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + + last_idle_time = system_load.tasks[0].total_runtime; + } + + + + /* if battery voltage is getting lower, warn using buzzer, etc. */ + if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + + if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { + low_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + tune_low_bat(); + } + + low_voltage_counter++; + } + + /* Critical, this is rather an emergency, change state machine */ + else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { + critical_battery_voltage_actions_done = true; + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + tune_critical_bat(); + // XXX implement state change here + } + + critical_voltage_counter++; + + } else { + low_voltage_counter = 0; + critical_voltage_counter = 0; + } + + /* End battery voltage check */ + + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + // XXX check for sensors + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + } else { + // XXX: Add emergency stuff if sensors are lost + } + + + /* + * Check for valid position information. + * + * If the system has a valid position source from an onboard + * position estimator, it is safe to operate it autonomously. + * The flag_vector_flight_mode_ok flag indicates that a minimum + * set of position measurements is available. + */ + + /* store current state to reason later about a state change */ + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + + + /* check for global or local position updates, set a timeout of 2s */ + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_global_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_global_position_valid = false; + } + + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_local_position_valid = true; + // XXX check for controller status and home position as well + + } else { + current_status.condition_local_position_valid = false; + } + + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { + current_status.condition_airspeed_valid = true; + + } else { + current_status.condition_airspeed_valid = false; + } + + /* + * Consolidate global position and local position valid flags + * for vector flight mode. + */ + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; + + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } + + /* consolidate state change, flag as changed if required */ + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { + state_changed = true; + } + + /* + * Mark the position of the first position lock as return to launch (RTL) + * position. The MAV will return here on command or emergency. + * + * Conditions: + * + * 1) The system aquired position lock just now + * 2) The system has not aquired position lock before + * 3) The system is not armed (on the ground) + */ + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it + + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } + + orb_check(gps_sub, &new_data); + if (new_data) { + + + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); + + /* check for first, long-term and valid GPS lock -> set home position */ + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; + + /* check if gps fix is ok */ + // XXX magic number + float hdop_threshold_m = 4.0f; + float vdop_threshold_m = 8.0f; + + /* + * If horizontal dilution of precision (hdop / eph) + * and vertical diluation of precision (vdop / epv) + * are below a certain threshold (e.g. 4 m), AND + * home position is not yet set AND the last GPS + * GPS measurement is not older than two seconds AND + * the system is currently not armed, set home + * position to the current position. + */ + + if (gps_position.fix_type == GPS_FIX_TYPE_3D + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !armed.armed) { + warnx("setting home position"); + + /* copy position data to uORB home message, store it locally as well */ + home.lat = gps_position.lat; + home.lon = gps_position.lon; + home.alt = gps_position.alt; + + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; + + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; + + /* announce new home position */ + if (home_pub > 0) { + orb_publish(ORB_ID(home_position), home_pub, &home); + } else { + home_pub = orb_advertise(ORB_ID(home_position), &home); + } + + /* mark home position as set */ + home_position_set = true; + tune_positive(); + } + } + + /* ignore RC signals if in offboard control mode */ + if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* Start RC state check */ + + if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + + warnx("mode sw not finite"); + /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; + + } else { + + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } + + // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + + /* + * Check if land/RTL is requested + */ + if (!isfinite(sp_man.return_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom stick position, set altitude hold */ + current_status.return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.return_switch = RETURN_SWITCH_RETURN; + + } else { + /* center stick position, set default */ + current_status.return_switch = RETURN_SWITCH_NONE; + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.mission_switch = MISSION_SWITCH_NONE; + + } else { + + /* center switch position, set default */ + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // XXX work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } + } + + /* + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { + + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { + mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); + tune_negative(); + } else { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); + } + + } else { + mavlink_log_critical(mavlink_fd, "DISARM not allowed"); + tune_negative(); + } + stick_off_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } + } + + /* check if left stick is in lower right position and we're in manual --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && + sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + stick_on_counter = 0; + tune_positive(); + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + } + + current_status.rc_signal_cutting_off = false; + current_status.rc_signal_lost = false; + current_status.rc_signal_lost_interval = 0; + + } else { + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* only complain if the offboard control is NOT active */ + current_status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + + /* if the RC signal is gone for a full second, consider it lost */ + if (current_status.rc_signal_lost_interval > 1000000) { + current_status.rc_signal_lost = true; + current_status.failsave_lowlevel = true; + state_changed = true; + } + + // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { + // publish_armed_status(¤t_status); + // } + } + } + + + + + /* End mode switch */ + + /* END RC state check */ + + + /* State machine update for offboard control */ + if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; + + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } + + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } + + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); + + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } + + current_status.offboard_control_signal_weak = false; + current_status.offboard_control_signal_lost = false; + current_status.offboard_control_signal_lost_interval = 0; + + // XXX check if this is correct + /* arm / disarm on request */ + if (sp_offboard.armed && !armed.armed) { + + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + + } else if (!sp_offboard.armed && armed.armed) { + + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + } + + } else { + + /* print error message for first RC glitch and then every 5 s / 5000 ms) */ + if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + current_status.offboard_control_signal_weak = true; + mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); + last_print_time = hrt_absolute_time(); + } + + /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ + current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + + /* if the signal is gone for 0.1 seconds, consider it lost */ + if (current_status.offboard_control_signal_lost_interval > 100000) { + current_status.offboard_control_signal_lost = true; + current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + tune_positive(); + + /* kill motors after timeout */ + if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + current_status.failsave_lowlevel = true; + state_changed = true; + } + } + } + } + + + + + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + + + // XXX this is missing + /* If full run came back clean, transition to standby */ + // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && + // current_status.flag_preflight_gyro_calibration == false && + // current_status.flag_preflight_mag_calibration == false && + // current_status.flag_preflight_accel_calibration == false) { + // /* All ok, no calibration going on, go to standby */ + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // } + + /* publish at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + state_changed = false; + } + + + + /* Store old modes to detect and act on state transitions */ + voltage_previous = current_status.battery_voltage; + + + /* play tone according to evaluation result */ + /* check if we recently armed */ + if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (tune_arm() == OK) + arm_tune_played = true; + + /* Trigger audio event for low battery */ + } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { + if (tune_critical_bat() == OK) + battery_tune_played = true; + } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { + if (tune_low_bat() == OK) + battery_tune_played = true; + } else if(battery_tune_played) { + tune_stop(); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + arm_tune_played = false; + } + + + /* XXX use this voltage_previous */ + fflush(stdout); + counter++; + usleep(COMMANDER_MONITORING_INTERVAL); + } + + /* wait for threads to complete */ + pthread_join(commander_low_prio_thread, NULL); + + /* close fds */ + led_deinit(); + buzzer_deinit(); + close(sp_man_sub); + close(sp_offboard_sub); + close(local_position_sub); + close(global_position_sub); + close(gps_sub); + close(sensor_sub); + close(safety_sub); + close(cmd_sub); + close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); + + warnx("exiting"); + fflush(stdout); + + thread_running = false; + + return 0; +} + + +void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + + // do_baro_calibration(mavlink_fd); + + case LOW_PRIO_TASK_RC_CALIBRATION: + + // do_rc_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} -- cgit v1.2.3 From 963abd66badf71a925f80e12312c429d64999424 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 21:48:21 +0400 Subject: commander: WIP, SEATBELT renamed to ASSISTED, added SIMPLE mode, added ASSISTED switch, some cleanup and reorganizing --- src/modules/commander/commander.cpp | 190 ++++++++++++---------- src/modules/commander/state_machine_helper.cpp | 66 ++++++-- src/modules/mavlink/mavlink.c | 12 +- src/modules/uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/vehicle_status.h | 17 +- 5 files changed, 178 insertions(+), 108 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 253b6295f..25c5a84ea 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { current_status.condition_global_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { current_status.condition_local_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_local_position_valid = false; @@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_SEATBELT; + current_status.mode_switch = MODE_SWITCH_ASSISTED; } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[]) /* this switch is not properly mapped, set default */ current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ current_status.return_switch = RETURN_SWITCH_RETURN; } else { - /* center stick position, set default */ + /* center or bottom stick position, set default */ current_status.return_switch = RETURN_SWITCH_NONE; } + /* check assisted switch */ + if (!isfinite(sp_man.assisted_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; + + } else { + /* center or bottom stick position, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { @@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } - /* Try seatbelt or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + /* Try assisted or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1235,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1250,89 +1259,104 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the navigation state when armed */ case ARMING_STATE_ARMED: - /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - + /* MANUAL */ if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } - /* SEATBELT_STANDBY (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_NONE) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* SEATBELT_DESCENT (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_RETURN) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_NONE) { - - /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { + /* ASSISTED */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* ASSISTED_DESCENT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); } } - /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + + } else { + if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { + /* ASSISTED_SIMPLE */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* ASSISTED_SEATBELT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } } - /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_MISSION) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + /* AUTO */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* AUTO_RTL */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_DESCENT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } - } - - /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_RETURN - && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else { + if (current_status.mission_switch == MISSION_SWITCH_MISSION) { + /* AUTO_MISSION */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + // TODO check this + if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* AUTO_LOITER */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cf60a99a..60ab01536 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions back to INIT are possible for calibration */ if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; @@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { @@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_STANDBY: + case NAVIGATION_STATE_ASSISTED_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { @@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT: + case NAVIGATION_STATE_ASSISTED_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_DESCENT: + case NAVIGATION_STATE_ASSISTED_SIMPLE: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_ASSISTED_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT or from other STANDBY modes or from AUTO READY */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { /* need to be disarmed and have a position and home lock */ @@ -395,7 +430,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ @@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ @@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index db8ee9067..ae8e168e1 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -221,9 +221,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { + if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -248,15 +248,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index cfee81ea2..eac6d6e98 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -58,6 +58,7 @@ struct manual_control_setpoint_s { float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ec08a6c13..9b91e834e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -60,12 +60,13 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_INIT = 0, NAVIGATION_STATE_MANUAL_STANDBY, NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT_STANDBY, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_ASSISTED_STANDBY, + NAVIGATION_STATE_ASSISTED_SEATBELT, + NAVIGATION_STATE_ASSISTED_SIMPLE, + NAVIGATION_STATE_ASSISTED_DESCENT, NAVIGATION_STATE_AUTO_STANDBY, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_AUTO_TAKEOFF, @@ -98,7 +99,7 @@ typedef enum { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_SEATBELT, + MODE_SWITCH_ASSISTED, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -107,6 +108,11 @@ typedef enum { RETURN_SWITCH_RETURN } return_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_SIMPLE +} assisted_switch_pos_t; + typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -179,6 +185,7 @@ struct vehicle_status_s mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; + assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; bool condition_battery_voltage_valid; -- cgit v1.2.3 From 7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 23 Jul 2013 14:56:12 +0400 Subject: commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed --- src/modules/commander/commander.cpp | 39 +++++++++----------------- src/modules/commander/state_machine_helper.cpp | 24 ++++++++++++---- src/modules/uORB/topics/vehicle_control_mode.h | 1 + 3 files changed, 33 insertions(+), 31 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 25c5a84ea..1978d8782 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) } } - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - + /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + * TODO allow disarm when landed + */ + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && + control_mode.flag_control_manual_enabled && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); stick_off_counter = 0; } else { @@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower right position and we're in manual mode --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 60ab01536..4a7aa66b7 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = false; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = false; } break; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } } @@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } break; @@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } break; @@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 8481a624f..d83eb45d9 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ -- cgit v1.2.3 From 8c1067a017714394955892e3159c8e0c61bd4ba1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Jul 2013 23:12:16 +0400 Subject: State machine rewritten, compiles, WIP --- src/modules/commander/commander.cpp | 1178 +++++++++++------------- src/modules/commander/state_machine_helper.cpp | 797 +++++++--------- src/modules/commander/state_machine_helper.h | 12 +- src/modules/mavlink/mavlink.c | 53 +- src/modules/uORB/topics/vehicle_status.h | 55 +- 5 files changed, 928 insertions(+), 1167 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1978d8782..b2336cbf6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -177,6 +177,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int */ int commander_thread_main(int argc, char *argv[]); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); + +transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -198,11 +204,11 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -242,244 +248,171 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: + case VEHICLE_CMD_DO_SET_MODE: + break; - /* request to activate HIL */ - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + break; - if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + break; - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - break; + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - /* request to disarm */ - } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } - } - - break; - - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - /* request for an autopilot reboot */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { - /* reboot is executed later, after positive tune is sent */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - tune_positive(); - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - tune_negative(); - } - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - break; + } - // /* request to land */ - // case VEHICLE_CMD_NAV_LAND: - // { - // //TODO: add check if landing possible - // //TODO: add landing maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } } - // break; - // - // /* request to takeoff */ - // case VEHICLE_CMD_NAV_TAKEOFF: - // { - // //TODO: add check if takeoff possible - // //TODO: add takeoff maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } - // } - // break; - // - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + result = VEHICLE_CMD_RESULT_DENIED; } - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { - /* accel calibration */ - if ((int)(cmd->param5) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + result = VEHICLE_CMD_RESULT_DENIED; } - } - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { + break; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; + case VEHICLE_CMD_PREFLIGHT_STORAGE: - case VEHICLE_CMD_PREFLIGHT_STORAGE: + if (((int)(cmd->param1)) == 0) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; - if (((int)(cmd->param1)) == 0) { + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else if (((int)(cmd->param1)) == 1) { - } else if (((int)(cmd->param1)) == 1) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } + break; default: @@ -546,12 +479,12 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + /* Main state machine */ - struct vehicle_status_s current_status; + struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); + memset(&status, 0, sizeof(status)); /* armed topic */ struct actuator_armed_s armed; @@ -566,17 +499,18 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); - current_status.navigation_state = NAVIGATION_STATE_INIT; - current_status.arming_state = ARMING_STATE_INIT; - current_status.hil_state = HIL_STATE_OFF; + status.main_state = MAIN_STATE_MANUAL; + status.navigation_state = NAVIGATION_STATE_STANDBY; + status.arming_state = ARMING_STATE_INIT; + status.hil_state = HIL_STATE_OFF; /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; + status.offboard_control_signal_found_once = false; + status.rc_signal_found_once = false; /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; + status.rc_signal_lost = true; + status.offboard_control_signal_lost = true; /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; @@ -585,31 +519,31 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - + status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; // XXX for now just set sensors as initialized - current_status.condition_system_sensors_initialized = true; + status.condition_system_sensors_initialized = true; // XXX just disable offboard control for now control_mode.flag_control_offboard_enabled = false; /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - + /* publish the new state */ - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ orb_advert_t home_pub = -1; @@ -686,7 +620,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&local_position, 0, sizeof(local_position)); uint64_t last_local_position_time = 0; - /* + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. @@ -747,23 +681,28 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + if (param_get(_param_sys_type, &(status.system_type)) != OK) { warnx("failed getting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + if (status.system_type == VEHICLE_TYPE_COAXIAL || + status.system_type == VEHICLE_TYPE_HELICOPTER || + status.system_type == VEHICLE_TYPE_TRICOPTER || + status.system_type == VEHICLE_TYPE_QUADROTOR || + status.system_type == VEHICLE_TYPE_HEXAROTOR || + status.system_type == VEHICLE_TYPE_OCTOROTOR) { control_mode.flag_external_manual_override_ok = false; + status.is_rotary_wing = true; } else { control_mode.flag_external_manual_override_ok = true; + status.is_rotary_wing = false; } /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); + param_get(_param_system_id, &(status.system_id)); + param_get(_param_component_id, &(status.component_id)); } } @@ -800,7 +739,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -830,34 +769,37 @@ int commander_thread_main(int argc, char *argv[]) /* set the condition to valid if there has recently been a local position estimate */ if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; + } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* update battery status */ orb_check(battery_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - current_status.battery_voltage = battery.voltage_v; - current_status.condition_battery_voltage_valid = true; + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - + } - if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { - current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } else { - current_status.battery_voltage = 0.0f; + status.battery_voltage = 0.0f; } /* update subsystem */ orb_check(subsys_sub, &new_data); - + if (new_data) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); @@ -865,26 +807,26 @@ int commander_thread_main(int argc, char *argv[]) /* mark / unmark as present */ if (info.present) { - current_status.onboard_control_sensors_present |= info.subsystem_type; + status.onboard_control_sensors_present |= info.subsystem_type; } else { - current_status.onboard_control_sensors_present &= ~info.subsystem_type; + status.onboard_control_sensors_present &= ~info.subsystem_type; } /* mark / unmark as enabled */ if (info.enabled) { - current_status.onboard_control_sensors_enabled |= info.subsystem_type; + status.onboard_control_sensors_enabled |= info.subsystem_type; } else { - current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + status.onboard_control_sensors_enabled &= ~info.subsystem_type; } /* mark / unmark as ok */ if (info.ok) { - current_status.onboard_control_sensors_health |= info.subsystem_type; + status.onboard_control_sensors_health |= info.subsystem_type; } else { - current_status.onboard_control_sensors_health &= ~info.subsystem_type; + status.onboard_control_sensors_health &= ~info.subsystem_type; } } @@ -899,6 +841,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { /* ready to arm */ led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); @@ -908,7 +851,7 @@ int commander_thread_main(int argc, char *argv[]) /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { /* GPS lock */ led_on(LED_BLUE); @@ -940,20 +883,20 @@ int commander_thread_main(int argc, char *argv[]) uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; } - + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; tune_low_bat(); } @@ -961,11 +904,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Critical, this is rather an emergency, change state machine */ - else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; tune_critical_bat(); // XXX implement state change here } @@ -980,9 +923,11 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (current_status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + //TODO publish state + } else { // XXX: Add emergency stuff if sensors are lost } @@ -999,32 +944,32 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.condition_global_position_valid; - bool local_pos_valid = current_status.condition_local_position_valid; - bool airspeed_valid = current_status.condition_airspeed_valid; + bool global_pos_valid = status.condition_global_position_valid; + bool local_pos_valid = status.condition_local_position_valid; + bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { - current_status.condition_global_position_valid = true; + status.condition_global_position_valid = true; } else { - current_status.condition_global_position_valid = false; + status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* Check for valid airspeed/differential pressure measurements */ if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_airspeed_valid = true; + status.condition_airspeed_valid = true; } else { - current_status.condition_airspeed_valid = false; + status.condition_airspeed_valid = false; } /* @@ -1040,9 +985,9 @@ int commander_thread_main(int argc, char *argv[]) // } /* consolidate state change, flag as changed if required */ - if (global_pos_valid != current_status.condition_global_position_valid || - local_pos_valid != current_status.condition_local_position_valid || - airspeed_valid != current_status.condition_airspeed_valid) { + if (global_pos_valid != status.condition_global_position_valid || + local_pos_valid != status.condition_local_position_valid || + airspeed_valid != status.condition_airspeed_valid) { state_changed = true; } @@ -1059,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) // if (!current_status.flag_valid_launch_position && // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it + // first time a valid position, store it and emit it // // XXX implement storage and publication of RTL position // current_status.flag_valid_launch_position = true; @@ -1068,6 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) // } orb_check(gps_sub, &new_data); + if (new_data) { @@ -1093,11 +1039,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !armed.armed) { + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1114,6 +1060,7 @@ int commander_thread_main(int argc, char *argv[]) /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); + } else { home_pub = orb_advertise(ORB_ID(home_position), &home); } @@ -1125,326 +1072,136 @@ int commander_thread_main(int argc, char *argv[]) } /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - + if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* start RC state check */ if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - - warnx("mode sw not finite"); - /* no valid stick position, go to default */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, go to manual mode */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.mode_switch = MODE_SWITCH_AUTO; + /* handle the case where RC signal was regained */ + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); } else { - - /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_ASSISTED; + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); + status.rc_signal_cutting_off = false; + status.rc_signal_lost = false; + status.rc_signal_lost_interval = 0; - /* - * Check if land/RTL is requested - */ - if (!isfinite(sp_man.return_switch)) { + transition_result_t res; // store all transitions results here - /* this switch is not properly mapped, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; + /* arm/disarm by RC */ + bool arming_state_changed; - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { + res = TRANSITION_NOT_CHANGED; - /* top stick position */ - current_status.return_switch = RETURN_SWITCH_RETURN; + /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); - } else { - /* center or bottom stick position, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - } + if (res == TRANSITION_CHANGED) + stick_off_counter = 0; - /* check assisted switch */ - if (!isfinite(sp_man.assisted_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; + } else { + stick_off_counter++; + stick_on_counter = 0; + } - } else { - /* center or bottom stick position, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + } else { + stick_off_counter = 0; + } } - /* check mission switch */ - if (!isfinite(sp_man.mission_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; + /* check if left stick is in lower right position and we're in manual mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + status.main_state == MAIN_STATE_MANUAL) { + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + if (res == TRANSITION_CHANGED) + stick_on_counter = 0; - /* top switch position */ - current_status.mission_switch = MISSION_SWITCH_MISSION; + } else { + stick_on_counter++; + stick_off_counter = 0; + } - } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + } else { + stick_on_counter = 0; + } + } - /* bottom switch position */ - current_status.mission_switch = MISSION_SWITCH_NONE; + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - } else { + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } - /* center switch position, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + tune_positive(); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } - /* Now it's time to handle the stick inputs */ - - switch (current_status.arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* just manual, XXX this might depend on the return switch */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - - /* Try assisted or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - - /* Try auto or fallback to seatbelt or even manual */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - /* MANUAL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - /* ASSISTED */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* ASSISTED_DESCENT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - } else { - if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { - /* ASSISTED_SIMPLE */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* ASSISTED_SEATBELT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - /* AUTO */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* AUTO_RTL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_DESCENT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - if (current_status.mission_switch == MISSION_SWITCH_MISSION) { - /* AUTO_MISSION */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - // TODO check this - if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* AUTO_LOITER */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - } - } - } - break; - - // XXX we might be missing something that triggers a transition from RTL to LAND - - case ARMING_STATE_ARMED_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); - } else { - if (current_status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } + /* evaluate the main state machine */ + res = check_main_state_machine(&status); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } - /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - * TODO allow disarm when landed - */ - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && - control_mode.flag_control_manual_enabled && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - stick_off_counter = 0; + bool main_state_changed = (res == TRANSITION_CHANGED); - } else { - stick_off_counter++; - stick_on_counter = 0; - } + /* evaluate the navigation state machine */ + res = check_navigation_state_machine(&status, &control_mode); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } - /* check if left stick is in lower right position and we're in manual mode --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - stick_on_counter = 0; - tune_positive(); + bool navigation_state_changed = (res == TRANSITION_CHANGED); - } else { - stick_on_counter++; - stick_off_counter = 0; - } + if (arming_state_changed || main_state_changed || navigation_state_changed) { + /* publish new vehicle status */ + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); } - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; + status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - if (!current_status.rc_signal_cutting_off) { + if (!status.rc_signal_cutting_off) { printf("Reason: not rc_signal_cutting_off\n"); + } else { printf("last print time: %llu\n", last_print_time); } @@ -1453,12 +1210,12 @@ int commander_thread_main(int argc, char *argv[]) } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; + if (status.rc_signal_lost_interval > 1000000) { + status.rc_signal_lost = true; + status.failsave_lowlevel = true; state_changed = true; } @@ -1477,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[]) /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // /* decide about attitude control flag, enable in att/pos/vel */ @@ -1518,42 +1275,44 @@ int commander_thread_main(int argc, char *argv[]) // } // } - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; + status.offboard_control_signal_weak = false; + status.offboard_control_signal_lost = false; + status.offboard_control_signal_lost_interval = 0; // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); + // TODO publish state } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + // TODO publish state } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + if (status.offboard_control_signal_lost_interval > 100000) { + status.offboard_control_signal_lost = true; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; + if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + status.failsave_lowlevel = true; state_changed = true; } } @@ -1563,8 +1322,8 @@ int commander_thread_main(int argc, char *argv[]) - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); + status.counter++; + status.timestamp = hrt_absolute_time(); // XXX this is missing @@ -1580,36 +1339,39 @@ int commander_thread_main(int argc, char *argv[]) /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); state_changed = false; } /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.battery_voltage; + voltage_previous = status.battery_voltage; /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { + /* Trigger audio event for low battery */ + + } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { + + } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; - } else if(battery_tune_played) { + + } else if (battery_tune_played) { tune_stop(); battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1647,6 +1409,172 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +{ + /* main mode switch */ + if (!isfinite(sp_man->mode_switch)) { + warnx("mode sw not finite"); + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_AUTO; + + } else { + current_status->mode_switch = MODE_SWITCH_MANUAL; + } + + /* land switch */ + if (!isfinite(sp_man->return_switch)) { + current_status->return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { + current_status->return_switch = RETURN_SWITCH_RETURN; + + } else { + current_status->return_switch = RETURN_SWITCH_NONE; + } + + /* assisted switch */ + if (!isfinite(sp_man->assisted_switch)) { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { + current_status->assisted_switch = ASSISTED_SWITCH_EASY; + + } else { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + + /* mission switch */ + if (!isfinite(sp_man->mission_switch)) { + current_status->mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { + current_status->mission_switch = MISSION_SWITCH_NONE; + + } else { + current_status->mission_switch = MISSION_SWITCH_MISSION; + } +} + +transition_result_t +check_main_state_machine(struct vehicle_status_s *current_status) +{ + /* evaluate the main state machine */ + transition_result_t res = TRANSITION_DENIED; + + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT + } + + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT (EASY likely will not work too) + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } + + return res; +} + +transition_result_t +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t res = TRANSITION_DENIED; + + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + /* ARMED */ + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + break; + + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + break; + + case MAIN_STATE_AUTO: + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + } else { + /* if not landed: act depending on switches */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + } + } + } + } + + break; + + default: + break; + } + + } else { + /* DISARMED */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + } + + return res; +} void *commander_low_prio_loop(void *arg) { @@ -1657,72 +1585,76 @@ void *commander_low_prio_loop(void *arg) switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + case LOW_PRIO_TASK_PARAM_LOAD: - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); - } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); - case LOW_PRIO_TASK_PARAM_SAVE: + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); - } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; - case LOW_PRIO_TASK_GYRO_CALIBRATION: + case LOW_PRIO_TASK_PARAM_SAVE: - do_gyro_calibration(mavlink_fd); + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } - case LOW_PRIO_TASK_MAG_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_mag_calibration(mavlink_fd); + case LOW_PRIO_TASK_GYRO_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_gyro_calibration(mavlink_fd); - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - // do_baro_calibration(mavlink_fd); + case LOW_PRIO_TASK_MAG_CALIBRATION: - case LOW_PRIO_TASK_RC_CALIBRATION: + do_mag_calibration(mavlink_fd); - // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; + break; - low_prio_task = LOW_PRIO_TASK_NONE; - break; + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - case LOW_PRIO_TASK_ACCEL_CALIBRATION: + // do_baro_calibration(mavlink_fd); - do_accel_calibration(mavlink_fd); + case LOW_PRIO_TASK_RC_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + // do_rc_calibration(mavlink_fd); - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_airspeed_calibration(mavlink_fd); + case LOW_PRIO_TASK_ACCEL_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_accel_calibration(mavlink_fd); - case LOW_PRIO_TASK_NONE: - default: - /* slow down to 10Hz */ - usleep(100000); - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4a7aa66b7..043ccfd0c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,102 +62,110 @@ #endif static const int ERROR = -1; -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { - - - int ret = ERROR; +transition_result_t +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_state->arming_state) { - ret = OK; + if (new_arming_state == status->arming_state) { + ret = TRANSITION_NOT_CHANGED; + } else { switch (new_arming_state) { - case ARMING_STATE_INIT: + case ARMING_STATE_INIT: - /* allow going back from INIT for calibration */ - if (current_state->arming_state == ARMING_STATE_STANDBY) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED) { - - /* sensors need to be initialized for STANDBY state */ - if (current_state->condition_system_sensors_initialized) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = true; - } else { - mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); - } - } - break; - case ARMING_STATE_ARMED: + /* allow going back from INIT for calibration */ + if (status->arming_state == ARMING_STATE_STANDBY) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + break; - /* XXX conditions for arming? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_ARMED_ERROR: - - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_ARMED) { - - /* XXX conditions for an error state? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_STANDBY_ERROR: - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_REBOOT: + case ARMING_STATE_STANDBY: - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + /* allow coming from INIT and disarming from ARMED */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED) { - ret = OK; + /* sensors need to be initialized for STANDBY state */ + if (status->condition_system_sensors_initialized) { + ret = TRANSITION_CHANGED; armed->armed = false; - armed->ready_to_arm = false; + armed->ready_to_arm = true; + } else { + mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } - break; - case ARMING_STATE_IN_AIR_RESTORE: + } - /* XXX implement */ - break; - default: - break; - } + break; + + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_ARMED) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } - if (ret == OK) { - current_state->arming_state = new_arming_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + break; - armed->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_armed), armed_pub, armed); + case ARMING_STATE_STANDBY_ERROR: + + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + hrt_abstime t = hrt_absolute_time(); + status->arming_state = new_arming_state; + armed->timestamp = t; } } @@ -165,400 +173,230 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta } - -/* - * This functions does not evaluate any input flags but only checks if the transitions - * are valid. - */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - - int ret = ERROR; +transition_result_t +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_state->navigation_state) { - ret = OK; + if (new_main_state == current_state->main_state) { + ret = TRANSITION_NOT_CHANGED; + } else { - switch (new_navigation_state) { - case NAVIGATION_STATE_INIT: - - /* transitions back to INIT are possible for calibration */ - if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { - - ret = OK; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_MANUAL_STANDBY: - - /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to be disarmed first */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - } - break; + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - case NAVIGATION_STATE_MANUAL: + case MAIN_STATE_SEATBELT: - /* need to be armed first */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - break; - - case NAVIGATION_STATE_ASSISTED_STANDBY: - - /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { - - /* need to be disarmed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SEATBELT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SIMPLE: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_DESCENT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_AUTO_STANDBY: - - /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* need to be disarmed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - - // XXX flag test needed? - - /* need to be armed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; + /* need position estimate */ + // TODO only need altitude estimate really + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); + tune_negative(); - case NAVIGATION_STATE_AUTO_TAKEOFF: + } else { + ret = TRANSITION_CHANGED; + } - /* only transitions from AUTO_READY */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { + break; - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_MISSION: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a mission ready */ - if (!current_state-> condition_auto_mission_available) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_LAND: - /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; + case MAIN_STATE_EASY: - default: - break; - } + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); + tune_negative(); + + } else { + ret = TRANSITION_CHANGED; + } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + break; + + case MAIN_STATE_AUTO: + + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); + tune_negative(); + + } else { + ret = TRANSITION_CHANGED; + } + + break; + } - control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + if (ret == TRANSITION_CHANGED) { + current_state->main_state = new_main_state; } } - + return ret; +} + +transition_result_t +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_STANDBY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_DIRECT: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_STABILIZE: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_ALTHOLD: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_VECTOR: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_READY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + current_status->navigation_state = new_navigation_state; + } + } return ret; } @@ -582,31 +420,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s switch (new_state) { - case HIL_STATE_OFF: + case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } - case HIL_STATE_ON: + break; - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + case HIL_STATE_ON: - current_control_mode->flag_system_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } - default: - warnx("Unknown hil state"); - break; + break; + + default: + warnx("Unknown hil state"); + break; } } @@ -621,6 +461,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); ret = OK; + } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 75fdd224c..b59b66c8b 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -49,10 +49,18 @@ #include #include +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); +} transition_result_t; -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ae8e168e1..9132d1b49 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } @@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { + if (v_status.main_state == MAIN_STATE_MANUAL + || v_status.main_state == MAIN_STATE_SEATBELT + || v_status.main_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { + if (v_status.navigation_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* set calibration state */ if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - - // XXX difference between active and armed? is AUTO_READY active? - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - + } else if (v_status.arming_state == ARMING_STATE_INIT + || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE + || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review + *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_ARMED) { *mavlink_state = MAV_STATE_ACTIVE; - - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { - + } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { - - *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; } else { - warnx("Unknown mavlink state"); *mavlink_state = MAV_STATE_CRITICAL; } - } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9b91e834e..e7feaa98e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -58,29 +58,28 @@ * @addtogroup topics @{ */ -/* State Machine */ +/* main state machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_MANUAL_STANDBY, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_ASSISTED_STANDBY, - NAVIGATION_STATE_ASSISTED_SEATBELT, - NAVIGATION_STATE_ASSISTED_SIMPLE, - NAVIGATION_STATE_ASSISTED_DESCENT, - NAVIGATION_STATE_AUTO_STANDBY, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_AUTO_TAKEOFF, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_LAND -} navigation_state_t; + MAIN_STATE_MANUAL = 0, + MAIN_STATE_SEATBELT, + MAIN_STATE_EASY, + MAIN_STATE_AUTO, +} main_state_t; +/* navigation state machine */ typedef enum { - MANUAL_STANDBY = 0, - MANUAL_READY, - MANUAL_IN_AIR -} manual_state_t; + NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed + NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_STABILIZE, // attitude stabilization + NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization + NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization + NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff + NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode + NAVIGATION_STATE_AUTO_LOITER, // pause mission + NAVIGATION_STATE_AUTO_MISSION, // fly mission + NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND + NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) +} navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, @@ -103,16 +102,16 @@ typedef enum { MODE_SWITCH_AUTO } mode_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_EASY +} assisted_switch_pos_t; + typedef enum { RETURN_SWITCH_NONE = 0, RETURN_SWITCH_RETURN } return_switch_pos_t; -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_SIMPLE -} assisted_switch_pos_t; - typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -175,7 +174,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current system state */ + main_state_t main_state; /**< main state machine */ + navigation_state_t navigation_state; /**< navigation state machine */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ @@ -183,6 +183,8 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ + bool is_rotary_wing; + mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; @@ -198,6 +200,7 @@ struct vehicle_status_s bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ -- cgit v1.2.3 From 02d4480e8ed7c6c6460f95f531aeef2725951663 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 31 Jul 2013 20:58:27 +0400 Subject: commander rewrite almost completed, WIP --- src/modules/commander/commander.cpp | 843 +++++++++++-------------- src/modules/commander/commander_helper.cpp | 6 +- src/modules/commander/state_machine_helper.cpp | 70 +- src/modules/commander/state_machine_helper.h | 12 +- 4 files changed, 445 insertions(+), 486 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b2336cbf6..2012abcc0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -73,7 +73,6 @@ #include #include #include -#include #include #include @@ -124,6 +123,26 @@ extern struct system_load_s system_load; #define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define PRINT_INTERVAL 5000000 +#define PRINT_MODE_REJECT_INTERVAL 2000000 + +// TODO include mavlink headers +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +enum MAV_MODE_FLAG { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END = 129, /* | */ +}; +#endif + /* Mavlink file descriptors */ static int mavlink_fd; @@ -135,9 +154,13 @@ static int daemon_task; /**< Handle of daemon task / thread */ /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; +static unsigned int leds_counter; +/* To remember when last notification was sent */ +static uint64_t last_print_mode_reject_time = 0; + /* tasks waiting for low prio thread */ -enum { +typedef enum { LOW_PRIO_TASK_NONE = 0, LOW_PRIO_TASK_PARAM_SAVE, LOW_PRIO_TASK_PARAM_LOAD, @@ -147,8 +170,9 @@ enum { LOW_PRIO_TASK_RC_CALIBRATION, LOW_PRIO_TASK_ACCEL_CALIBRATION, LOW_PRIO_TASK_AIRSPEED_CALIBRATION -} low_prio_task; +} low_prio_task_t; +static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE; /** * The daemon app only briefly exists to start @@ -170,17 +194,21 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. */ int commander_thread_main(int argc, char *argv[]); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +void print_reject_mode(const char *msg); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); /** @@ -241,141 +269,112 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: - break; - - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - break; + case VEHICLE_CMD_DO_SET_MODE: { + uint8_t base_mode = (int) cmd->param1; + uint8_t custom_mode = (int) cmd->param2; + // TODO remove debug code + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + /* set arming state */ + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); + } - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - break; + } else { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); + } + } else { + arming_res = TRANSITION_NOT_CHANGED; + } + } - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + if (custom_mode & 1) { // TODO add custom mode flags definition + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else { - result = VEHICLE_CMD_RESULT_DENIED; + } else { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } } + } + + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { + break; + } - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + break; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + break; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } + if ((int)(cmd->param1) == 1) { + /* gyro calibration */ + new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if ((int)(cmd->param2) == 1) { + /* magnetometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else if ((int)(cmd->param3) == 1) { + /* zero-altitude pressure calibration */ + //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + } else if ((int)(cmd->param4) == 1) { + /* RC calibration */ + new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + } else if ((int)(cmd->param5) == 1) { + /* accelerometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else if ((int)(cmd->param6) == 1) { + /* airspeed calibration */ + new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; } - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state + if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -384,54 +383,55 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - break; + break; + } - case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - if (((int)(cmd->param1)) == 0) { - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + if (((int)(cmd->param1)) == 0) { low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else if (((int)(cmd->param1)) == 1) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { result = VEHICLE_CMD_RESULT_ACCEPTED; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - break; + break; + } default: - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; break; } /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED || - result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + } else { tune_negative(); - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); - tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); + } } /* send any requested ACKs */ @@ -515,16 +515,9 @@ int commander_thread_main(int argc, char *argv[]) /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; - /* flag position info as bad, do not allow auto mode */ - // current_status.flag_vector_flight_mode_ok = false; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ - /* XXX do we need this? */ - //current_status.flag_safety_present = false; - // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -535,12 +528,11 @@ int commander_thread_main(int argc, char *argv[]) status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - /* publish the new state */ + /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -556,8 +548,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } - // XXX needed? - mavlink_log_info(mavlink_fd, "system is running"); + mavlink_log_info(mavlink_fd, "[cmd] started"); pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); @@ -577,9 +568,10 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_on_counter = 0; /* To remember when last notification was sent */ - uint64_t last_print_time = 0; + uint64_t last_print_control_time = 0; - float voltage_previous = 0.0f; + enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; + bool armed_previous = false; bool low_battery_voltage_actions_done; bool critical_battery_voltage_actions_done; @@ -588,10 +580,10 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = 0; - bool state_changed = true; + bool status_changed = true; bool param_init_forced = true; - bool new_data = false; + bool updated = false; /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -612,13 +604,11 @@ int commander_thread_main(int argc, char *argv[]) int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; /* * The home position is set based on GPS only, to prevent a dependency between @@ -670,11 +660,12 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { + hrt_abstime t = hrt_absolute_time(); /* update parameters */ - orb_check(param_changed_sub, &new_data); + orb_check(param_changed_sub, &updated); - if (new_data || param_init_forced) { + if (updated || param_init_forced) { param_init_forced = false; /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); @@ -703,94 +694,96 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - + status_changed = true; } } - orb_check(sp_man_sub, &new_data); + orb_check(sp_man_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &new_data); + orb_check(sp_offboard_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(sensor_sub, &new_data); + orb_check(sensor_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } - orb_check(diff_pres_sub, &new_data); + orb_check(diff_pres_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); last_diff_pres_time = diff_pres.timestamp; } - orb_check(cmd_sub, &new_data); + orb_check(cmd_sub, &updated); - if (new_data) { + if (updated) { /* got command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(&status, &control_mode, &cmd, &armed); } /* update safety topic */ - orb_check(safety_sub, &new_data); + orb_check(safety_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ - orb_check(global_position_sub, &new_data); + orb_check(global_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; } /* update local position estimate */ - orb_check(local_position_sub, &new_data); + orb_check(local_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; } /* set the condition to valid if there has recently been a local position estimate */ - if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - status.condition_local_position_valid = true; + if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (!status.condition_local_position_valid) { + status.condition_local_position_valid = true; + status_changed = true; + } } else { - status.condition_local_position_valid = false; + if (status.condition_local_position_valid) { + status.condition_local_position_valid = false; + status_changed = true; + } } /* update battery status */ - orb_check(battery_sub, &new_data); + orb_check(battery_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - } - if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); } else { @@ -798,12 +791,12 @@ int commander_thread_main(int argc, char *argv[]) } /* update subsystem */ - orb_check(subsys_sub, &new_data); + orb_check(subsys_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("Subsys changed: %d\n", (int)info.subsystem_type); + warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { @@ -828,56 +821,12 @@ int commander_thread_main(int argc, char *argv[]) } else { status.onboard_control_sensors_health &= ~info.subsystem_type; } - } - - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - - /* XXX if armed */ - if (armed.armed) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { - /* ready to arm */ - led_toggle(LED_AMBER); - - } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not ready to arm, something is wrong */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - - // /* toggle GPS led at 5 Hz in HIL mode */ - // if (current_status.flag_hil_enabled) { - // /* hil enabled */ - // led_toggle(LED_BLUE); - - // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - // /* toggle arming (red) at 5 Hz on low battery or error */ - // led_toggle(LED_AMBER); - // } + status_changed = true; } + toggle_status_leds(&status, &armed, &gps_position); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -888,29 +837,26 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - tune_low_bat(); + status_changed = true; } low_voltage_counter++; - } - /* Critical, this is rather an emergency, change state machine */ - else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - tune_critical_bat(); - // XXX implement state change here + arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + status_changed = true; } critical_voltage_counter++; @@ -923,10 +869,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - //TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -950,14 +895,14 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { + if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { status.condition_global_position_valid = true; } else { status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { + if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { status.condition_local_position_valid = true; } else { @@ -965,66 +910,23 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { + if (t - last_diff_pres_time < 2000000 && t > 2000000) { status.condition_airspeed_valid = true; } else { status.condition_airspeed_valid = false; } - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - // if (current_status.condition_local_position_valid || - // current_status.condition_global_position_valid) { - // current_status.flag_vector_flight_mode_ok = true; - - // } else { - // current_status.flag_vector_flight_mode_ok = false; - // } - - /* consolidate state change, flag as changed if required */ - if (global_pos_valid != status.condition_global_position_valid || - local_pos_valid != status.condition_local_position_valid || - airspeed_valid != status.condition_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - // if (!current_status.flag_valid_launch_position && - // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it - - // // XXX implement storage and publication of RTL position - // current_status.flag_valid_launch_position = true; - // current_status.flag_auto_flight_mode_ok = true; - // state_changed = true; - // } - - orb_check(gps_sub, &new_data); - - if (new_data) { - + orb_check(gps_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ float hdop_m = gps_position.eph_m; float vdop_m = gps_position.epv_m; - /* check if gps fix is ok */ - // XXX magic number + /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -1038,15 +940,12 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (t - gps_position.timestamp_position < 2000000) && !armed.armed) { - warnx("setting home position"); - /* copy position data to uORB home message, store it locally as well */ + // TODO use global position estimate home.lat = gps_position.lat; home.lon = gps_position.lon; home.alt = gps_position.alt; @@ -1057,6 +956,11 @@ int commander_thread_main(int argc, char *argv[]) home.s_variance_m_s = gps_position.s_variance_m_s; home.p_variance_m = gps_position.p_variance_m; + double home_lat_d = home.lat * 1e-7; + double home_lon_d = home.lon * 1e-7; + warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1073,16 +977,18 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC state check */ - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + /* start RC input check */ + if ((t - sp_man.timestamp) < 100000) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + status_changed = true; } } @@ -1093,8 +999,6 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res; // store all transitions results here /* arm/disarm by RC */ - bool arming_state_changed; - res = TRANSITION_NOT_CHANGED; /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm @@ -1106,16 +1010,15 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_off_counter = 0; + res = arming_state_transition(&status, new_arming_state, &armed); + stick_off_counter = 0; } else { stick_off_counter++; - stick_on_counter = 0; } + stick_on_counter = 0; + } else { stick_off_counter = 0; } @@ -1126,16 +1029,15 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_on_counter = 0; + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { stick_on_counter++; - stick_off_counter = 0; } + stick_off_counter = 0; + } else { stick_on_counter = 0; } @@ -1148,9 +1050,6 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - - tune_positive(); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* fill current_status according to mode switches */ @@ -1159,83 +1058,54 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine */ res = check_main_state_machine(&status); - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - } - - bool main_state_changed = (res == TRANSITION_CHANGED); - - /* evaluate the navigation state machine */ - res = check_navigation_state_machine(&status, &control_mode); - - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - } - - bool navigation_state_changed = (res == TRANSITION_CHANGED); - - if (arming_state_changed || main_state_changed || navigation_state_changed) { - /* publish new vehicle status */ - status.counter++; - status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); - mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - } + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); - if (navigation_state_changed) { - /* publish new navigation state */ - control_mode.counter++; - control_mode.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - + /* print error message for first RC glitch and then every 5s */ + if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + // TODO remove debug code if (!status.rc_signal_cutting_off) { - printf("Reason: not rc_signal_cutting_off\n"); + warnx("Reason: not rc_signal_cutting_off\n"); } else { - printf("last print time: %llu\n", last_print_time); + warnx("last print time: %llu\n", last_print_control_time); } - last_print_time = hrt_absolute_time(); + /* only complain if the offboard control is NOT active */ + status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); + + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = t - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { status.rc_signal_lost = true; status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } } } - - - - /* End mode switch */ - + /* END mode switch */ /* END RC state check */ - - /* State machine update for offboard control */ + // TODO check this + /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1282,87 +1152,94 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - - arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* print error message for first offboard signal glitch and then every 5s */ + if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); + status.failsave_lowlevel_start_time = t; tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } } } } + /* evaluate the navigation state machine */ + transition_result_t res = check_navigation_state_machine(&status, &control_mode); + if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + } + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = check_arming_state_changed(); + bool main_state_changed = check_main_state_changed(); + bool navigation_state_changed = check_navigation_state_changed(); - status.counter++; - status.timestamp = hrt_absolute_time(); - + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; + } - // XXX this is missing - /* If full run came back clean, transition to standby */ - // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - // current_status.flag_preflight_gyro_calibration == false && - // current_status.flag_preflight_mag_calibration == false && - // current_status.flag_preflight_accel_calibration == false) { - // /* All ok, no calibration going on, go to standby */ - // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - // } + /* publish arming state */ + if (arming_state_changed) { + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { + /* publish control mode */ + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } + /* publish vehicle status at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + status.counter++; + status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - state_changed = false; + status_changed = false; } - - - /* Store old modes to detect and act on state transitions */ - voltage_previous = status.battery_voltage; - - - /* play tone according to evaluation result */ - /* check if we recently armed */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + /* play arming and battery warning tunes */ + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { + /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - - } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { - if (tune_critical_bat() == OK) + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + /* play tune on battery warning */ + if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { - if (tune_low_bat() == OK) + } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + /* play tune on battery critical */ + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (battery_tune_played) { @@ -1375,8 +1252,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + /* store old modes to detect and act on state transitions */ + battery_warning_previous = status.battery_warning; + armed_previous = armed.armed; - /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -1409,6 +1288,46 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +{ + if (leds_counter % 2 == 0) { + /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 2.5Hz */ + if (leds_counter % 8 == 0) { + led_toggle(LED_AMBER); + } + + } else { + /* not ready to arm, blink at 10Hz */ + led_toggle(LED_AMBER); + } + + if (status->condition_global_position_valid) { + /* position estimated, solid */ + led_on(LED_BLUE); + + } else if (leds_counter == 0) { + /* waiting for position estimate, short blink at 1.25Hz */ + led_on(LED_BLUE); + + } else { + /* no position estimator available, off */ + led_off(LED_BLUE); + } + } + + leds_counter++; + + if (leds_counter >= 16) + leds_counter = 0; +} + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) { @@ -1420,8 +1339,11 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_AUTO; - } else { + } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else { + current_status->mode_switch = MODE_SWITCH_ASSISTED; } /* land switch */ @@ -1466,44 +1388,49 @@ check_main_state_machine(struct vehicle_status_s *current_status) switch (current_status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT + print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1514,6 +1441,20 @@ check_main_state_machine(struct vehicle_status_s *current_status) return res; } +void +print_reject_mode(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] WARNING: reject %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { @@ -1523,15 +1464,15 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* ARMED */ switch (current_status->main_state) { case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); break; case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); break; case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); break; case MAIN_STATE_AUTO: @@ -1539,7 +1480,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* don't act while taking off */ if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); // TRANSITION_DENIED is not possible here break; @@ -1547,16 +1488,16 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* if not landed: act depending on switches */ if (current_status->return_switch == RETURN_SWITCH_RETURN) { /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { if (current_status->mission_switch == MISSION_SWITCH_MISSION) { /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } @@ -1570,7 +1511,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v } else { /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); } return res; @@ -1579,75 +1520,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void *commander_low_prio_loop(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "commander low prio", getpid()); + prctl(PR_SET_NAME, "commander_low_prio", getpid()); while (!thread_should_exit) { switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1657,6 +1584,8 @@ void *commander_low_prio_loop(void *arg) break; } + low_prio_task = LOW_PRIO_TASK_NONE; + } return 0; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 9427bd892..681a11568 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -213,7 +213,7 @@ float battery_remaining_estimate_voltage(float voltage) ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; + ret = (ret < 0.0f) ? 0.0f : ret; + ret = (ret > 1.0f) ? 1.0f : ret; return ret; -} \ No newline at end of file +} diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 043ccfd0c..06cd060c5 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,8 +62,12 @@ #endif static const int ERROR = -1; +static bool arming_state_changed = true; +static bool main_state_changed = true; +static bool navigation_state_changed = true; + transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -96,9 +100,6 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi ret = TRANSITION_CHANGED; armed->armed = false; armed->ready_to_arm = true; - - } else { - mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } } @@ -163,18 +164,28 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi } if (ret == TRANSITION_CHANGED) { - hrt_abstime t = hrt_absolute_time(); status->arming_state = new_arming_state; - armed->timestamp = t; + arming_state_changed = true; } } return ret; } +bool +check_arming_state_changed() +{ + if (arming_state_changed) { + arming_state_changed = false; + return true; + + } else { + return false; + } +} transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; @@ -193,11 +204,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need position estimate */ // TODO only need altitude estimate really - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -206,11 +213,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -219,11 +222,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -232,14 +231,27 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m if (ret == TRANSITION_CHANGED) { current_state->main_state = new_main_state; + main_state_changed = true; } } return ret; } +bool +check_main_state_changed() +{ + if (main_state_changed) { + main_state_changed = false; + return true; + + } else { + return false; + } +} + transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; @@ -395,12 +407,24 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + navigation_state_changed = true; } } return ret; } +bool +check_navigation_state_changed() +{ + if (navigation_state_changed) { + navigation_state_changed = false; + return true; + + } else { + return false; + } +} /** * Transition from one hil state to another diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b59b66c8b..c8c77e5d8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,11 +56,17 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); -transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); +bool check_arming_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); + +bool check_main_state_changed(); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); + +bool check_navigation_state_changed(); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); -- cgit v1.2.3 From 32439d748ad169f6f9956fb3248535730e0374a4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 7 Aug 2013 20:21:49 +0200 Subject: commander: set mode using base_mode and custom_mode --- src/modules/commander/commander.cpp | 58 ++++++++++++++++++++++++++----------- 1 file changed, 41 insertions(+), 17 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2012abcc0..7ede3e1e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -126,10 +126,6 @@ extern struct system_load_s system_load; #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 -// TODO include mavlink headers -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -141,7 +137,13 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -#endif + +enum PX4_CUSTOM_MODE { + PX4_CUSTOM_MODE_MANUAL = 1, + PX4_CUSTOM_MODE_SEATBELT, + PX4_CUSTOM_MODE_EASY, + PX4_CUSTOM_MODE_AUTO, +}; /* Mavlink file descriptors */ static int mavlink_fd; @@ -277,8 +279,9 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (int) cmd->param1; - uint8_t custom_mode = (int) cmd->param2; + uint8_t base_mode = (uint8_t) cmd->param1; + uint32_t custom_mode = (uint32_t) cmd->param2; + // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); /* set arming state */ @@ -286,6 +289,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); } @@ -294,9 +298,11 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); } + } else { arming_res = TRANSITION_NOT_CHANGED; } @@ -305,21 +311,37 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + + } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - if (custom_mode & 1) { // TODO add custom mode flags definition - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } - } else { + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); } @@ -1534,6 +1556,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } + break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1545,6 +1568,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } + break; case LOW_PRIO_TASK_GYRO_CALIBRATION: -- cgit v1.2.3 From d7730a3444b1c4277bca24988402839a98a52fdc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 10:59:22 +0200 Subject: commander, mavlink: fixed base_mode and custom_mode in mavlink --- src/modules/commander/commander.cpp | 14 ++++------ src/modules/mavlink/mavlink.c | 52 ++++++++++++++++++------------------- src/modules/mavlink/orb_listener.c | 22 +++++++++------- src/modules/mavlink/util.h | 2 +- 4 files changed, 43 insertions(+), 47 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ede3e1e6..82b575405 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include +#include "px4_custom_mode.h" #include "commander_helper.h" #include "state_machine_helper.h" #include "calibration_routines.h" @@ -138,13 +139,6 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, -}; - /* Mavlink file descriptors */ static int mavlink_fd; @@ -1321,8 +1315,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } else if (armed->ready_to_arm) { /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + if (leds_counter & 8) { + led_on(LED_AMBER); + } else { + led_off(LED_AMBER); } } else { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9132d1b49..6d9ca1120 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,6 +64,7 @@ #include #include #include +#include #include "waypoints.h" #include "orb_topics.h" @@ -181,10 +182,11 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; /** * Set mode flags @@ -192,36 +194,31 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* HIL */ if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* autonomous mode */ - if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; - } - - /* set arming state */ + /* arming state */ if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.main_state == MAIN_STATE_MANUAL - || v_status.main_state == MAIN_STATE_SEATBELT - || v_status.main_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; } - if (v_status.navigation_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - /** * Set mavlink state **/ @@ -645,11 +642,12 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ if (v_status.hil_state == HIL_STATE_ON) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d088d421e..2a260861d 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -279,15 +279,16 @@ l_vehicle_status(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, - mavlink_mode, - v_status.navigation_state, + mavlink_base_mode, + mavlink_custom_mode, mavlink_state); } @@ -473,8 +474,9 @@ l_actuator_outputs(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -491,7 +493,7 @@ l_actuator_outputs(const struct listener *l) -1, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { @@ -505,7 +507,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { @@ -519,7 +521,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, + mavlink_base_mode, 0); } else { @@ -533,7 +535,7 @@ l_actuator_outputs(const struct listener *l) (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, + mavlink_base_mode, 0); } } diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index a4ff06a88..5e5ee8261 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); -- cgit v1.2.3 From cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Aug 2013 09:52:22 +0200 Subject: Rejecting arming if safety switch is not in safe position, added reboot command --- src/modules/commander/commander.cpp | 49 ++++++++++++++++++-------- src/modules/commander/state_machine_helper.cpp | 21 +++++++++-- src/modules/commander/state_machine_helper.h | 6 +++- 3 files changed, 58 insertions(+), 18 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b575405..d4c2c4c84 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -265,7 +265,7 @@ void usage(const char *reason) exit(1); } -void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -282,7 +282,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); @@ -291,7 +291,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -356,6 +356,23 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(status, safety, armed)) { + + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -388,7 +405,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* check if we have new task and no other task is scheduled */ if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { + if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; low_prio_task = new_low_prio_task; @@ -407,10 +424,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ @@ -605,6 +622,8 @@ int commander_thread_main(int argc, char *argv[]) int safety_sub = orb_subscribe(ORB_ID(safety)); struct safety_s safety; memset(&safety, 0, sizeof(safety)); + safety.safety_switch_available = false; + safety.safety_off = false; /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -746,7 +765,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &control_mode, &cmd, &armed); + handle_command(&status, &safety, &control_mode, &cmd, &armed); } /* update safety topic */ @@ -871,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); status_changed = true; } @@ -887,7 +906,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1026,7 +1045,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1045,7 +1064,7 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); stick_on_counter = 0; } else { @@ -1168,10 +1187,10 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } } else { @@ -1243,7 +1262,7 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { + if (!arm_tune_played && armed.armed) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1540,6 +1559,8 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); + low_prio_task = LOW_PRIO_TASK_NONE; + while (!thread_should_exit) { switch (low_prio_task) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 06cd060c5..163aceed2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -108,8 +108,10 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + if ((status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ + { ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -172,6 +174,19 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi return ret; } +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + } else { + return false; + } +} + bool check_arming_state_changed() { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index c8c77e5d8..a38c2497e 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include #include typedef enum { @@ -56,7 +57,10 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed); + +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); bool check_arming_state_changed(); -- cgit v1.2.3 From e5af29be1706b1d20d6bafe8c481213c76cc0d34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 12:38:25 +0200 Subject: Fixed param save and load --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b575405..5b3654bcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -407,10 +407,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ -- cgit v1.2.3 From 39ae01dd07d53e3509826ae3737fc6a509adec34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 13:29:42 +0200 Subject: Fix the low priority loop, calibration routines work again --- src/modules/commander/commander.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5b3654bcd..c6e209f0f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1552,7 +1552,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1564,37 +1564,43 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1604,8 +1610,6 @@ void *commander_low_prio_loop(void *arg) break; } - low_prio_task = LOW_PRIO_TASK_NONE; - } return 0; -- cgit v1.2.3 From f51320d1afac836085ee4d9dbdaeda7af18bcbda Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:27:29 +0200 Subject: Lov voltage warning should work again --- src/modules/commander/commander.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 41e22d21d..71f33d81b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -550,6 +550,7 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -606,8 +607,8 @@ int commander_thread_main(int argc, char *argv[]) enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; bool armed_previous = false; - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; @@ -810,19 +811,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - status.battery_voltage = battery.voltage_v; - status.condition_battery_voltage_valid = true; - } - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + warnx("bat v: %2.2f", battery.voltage_v); - } else { - status.battery_voltage = 0.0f; + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } } /* update subsystem */ -- cgit v1.2.3 From 98f403002f72e7fb3e38398de9d87746f7918347 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:27:29 +0200 Subject: Lov voltage warning should work again --- src/modules/commander/commander.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5de99040c..1ae88d17a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -550,6 +550,7 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -606,8 +607,8 @@ int commander_thread_main(int argc, char *argv[]) enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; bool armed_previous = false; - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; @@ -810,19 +811,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - status.battery_voltage = battery.voltage_v; - status.condition_battery_voltage_valid = true; - } - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + warnx("bat v: %2.2f", battery.voltage_v); - } else { - status.battery_voltage = 0.0f; + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } } /* update subsystem */ -- cgit v1.2.3 From d75c3c4e7369510db1d91721c2793c23dcd873fa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:48:28 +0200 Subject: Try to open RGBLEDs in commander (WIP) --- src/modules/commander/commander.cpp | 9 ++---- src/modules/commander/commander_helper.cpp | 45 +++++++++++++++++++++++------- 2 files changed, 38 insertions(+), 16 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 71f33d81b..4e6ecd1e4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1331,11 +1331,11 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } else if (armed->ready_to_arm) { /* ready to arm, blink at 2.5Hz */ - if (leds_counter & 8) { - led_on(LED_AMBER); + if (leds_counter % 8 == 0) { + led_toggle(LED_AMBER); } else { - led_off(LED_AMBER); + led_toggle(LED_AMBER); } } else { @@ -1358,9 +1358,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } leds_counter++; - - if (leds_counter >= 16) - leds_counter = 0; } void diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 681a11568..d9b255f4f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -53,6 +53,8 @@ #include #include #include +#include + #include "commander_helper.h" @@ -136,9 +138,11 @@ void tune_stop() } static int leds; +static int rgbleds; int led_init() { + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); if (leds < 0) { @@ -146,10 +150,29 @@ int led_init() return ERROR; } - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); + /* the blue LED is only available on FMUv1 but not FMUv2 */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + + if (ioctl(leds, LED_ON, LED_BLUE)) { + warnx("Blue LED: ioctl fail\n"); return ERROR; } +#endif + + if (ioctl(leds, LED_ON, LED_AMBER)) { + warnx("Amber LED: ioctl fail\n"); + return ERROR; + } + + /* then try RGB LEDs, this can fail on FMUv1*/ + rgbleds = open(RGBLED_DEVICE_PATH, 0); + if (rgbleds == -1) { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + errx(1, "Unable to open " RGBLED_DEVICE_PATH); +#else + warnx("No RGB LED found"); +#endif + } return 0; } @@ -157,18 +180,15 @@ int led_init() void led_deinit() { close(leds); + + if (rgbleds != -1) { + close(rgbleds); + } } int led_toggle(int led) { - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); + return ioctl(leds, LED_TOGGLE, led); } int led_on(int led) @@ -181,6 +201,11 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } +int rgbled_set_color(rgbled_color_t color) { + + return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color); +} + float battery_remaining_estimate_voltage(float voltage) { float ret = 0; -- cgit v1.2.3 From 3f9f1d60e03f501209deb6c7532c37dfb786f343 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 09:23:39 +0200 Subject: Added audio and text warning if arming is refused due to mode switch --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ae88d17a..6181dafab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -204,6 +204,7 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); +void print_reject_arm(const char *msg); transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); @@ -1082,6 +1083,16 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates safety switch not pressed */ + + if (!(!safety.safety_switch_available || safety.safety_off)) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } } /* fill current_status according to mode switches */ @@ -1490,6 +1501,20 @@ print_reject_mode(const char *msg) } } +void +print_reject_arm(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { -- cgit v1.2.3 From ec9de4ad84be8e62b762567c58ec3bb948684b43 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 09:27:05 +0200 Subject: Critical voltage now leads to a proper arming state transition --- src/modules/commander/commander.cpp | 10 +++++++--- src/modules/commander/state_machine_helper.cpp | 2 ++ 2 files changed, 9 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4e6ecd1e4..926be91b9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -812,8 +812,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { status.battery_voltage = battery.voltage_v; @@ -887,7 +885,13 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + + if (armed.armed) { + // XXX not sure what should happen when voltage is low in flight + //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + } else { + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + } status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2..ef3890b71 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -168,6 +168,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * if (ret == TRANSITION_CHANGED) { status->arming_state = new_arming_state; arming_state_changed = true; + } else { + warnx("arming transition rejected"); } } -- cgit v1.2.3 From 02c23c785e5aa5d85f737a7735bc62355f945066 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 10:17:57 +0200 Subject: System status load is now from 0 to 1 instead of non-intuitive 0 to 1000 --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 926be91b9..fc7670ee5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -862,7 +862,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) - status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 6d9ca1120..3d3434670 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -660,7 +660,7 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_present, v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, - v_status.load, + v_status.load * 1000.0f, v_status.battery_voltage * 1000.0f, v_status.battery_current * 1000.0f, v_status.battery_remaining, diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index e7feaa98e..4d49316c3 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -224,7 +224,7 @@ struct vehicle_status_s uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; - float load; + float load; /**< processor load from 0 to 1 */ float battery_voltage; float battery_current; float battery_remaining; -- cgit v1.2.3 From 451adf2aa0d9795f69f5675b00ff3fb245312eb0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 16 Aug 2013 18:05:59 +0200 Subject: Added part of RGBLED stuff to commander --- src/modules/commander/commander.cpp | 114 ++++++++++++++++++++++------- src/modules/commander/commander_helper.cpp | 27 +++++-- src/modules/commander/commander_helper.h | 9 ++- 3 files changed, 116 insertions(+), 34 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fc7670ee5..2e5d080b9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -197,7 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -698,6 +698,8 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { hrt_abstime t = hrt_absolute_time(); + status_changed = false; + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -855,8 +857,6 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - toggle_status_leds(&status, &armed, &gps_position); - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -867,14 +867,17 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } + warnx("bat remaining: %2.2f", status.battery_remaining); + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; status_changed = true; + battery_tune_played = false; } low_voltage_counter++; @@ -885,12 +888,14 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + battery_tune_played = false; if (armed.armed) { // XXX not sure what should happen when voltage is low in flight //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + // XXX should we still allow to arm with critical battery? + //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; } @@ -1259,7 +1264,6 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - status_changed = false; } /* play arming and battery warning tunes */ @@ -1273,7 +1277,7 @@ int commander_thread_main(int argc, char *argv[]) if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1294,6 +1298,9 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; + + toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1325,40 +1332,93 @@ int commander_thread_main(int argc, char *argv[]) } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { - if (leds_counter % 2 == 0) { - /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_BLUE); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 1Hz */ + if (leds_counter % 20 == 0) + led_toggle(LED_BLUE); + } else { + /* not ready to arm, blink at 10Hz */ + if (leds_counter % 2 == 0) + led_toggle(LED_BLUE); + } +#endif + + if (changed) { + + warnx("changed"); + + int i; + rgbled_pattern_t pattern; + memset(&pattern, 0, sizeof(pattern)); + if (armed->armed) { /* armed, solid */ - led_on(LED_AMBER); + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[0] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[0] = RGBLED_COLOR_RED; + } else { + pattern.color[0] = RGBLED_COLOR_GREEN; + } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + for (i=0; i<3; i++) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + pattern.color[i*2] = RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + pattern.color[i*2] = RGBLED_COLOR_RED; + } else { + pattern.color[i*2] = RGBLED_COLOR_GREEN; + } + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; + } + if (status->condition_global_position_valid) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 1000; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 800; } else { - led_toggle(LED_AMBER); + for (i=3; i<6; i++) { + pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.duration[i*2] = 100; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 100; + } + pattern.color[6*2] = RGBLED_COLOR_OFF; + pattern.duration[6*2] = 700; } } else { + for (i=0; i<3; i++) { + pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.duration[i*2] = 200; + pattern.color[i*2+1] = RGBLED_COLOR_OFF; + pattern.duration[i*2+1] = 200; + } /* not ready to arm, blink at 10Hz */ - led_toggle(LED_AMBER); } - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 0) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); + rgbled_set_pattern(&pattern); + } - } else { - /* no position estimator available, off */ - led_off(LED_BLUE); - } + /* give system warnings on error LED, XXX maybe add memory usage warning too */ + if (status->load > 0.95f) { + if (leds_counter % 2 == 0) + led_toggle(LED_AMBER); + } else { + led_off(LED_AMBER); } leds_counter++; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index d9b255f4f..5df5d8d0c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -122,16 +122,18 @@ int tune_arm() return ioctl(buzzer, TONE_SET_ALARM, 12); } -int tune_critical_bat() +int tune_low_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 14); + return ioctl(buzzer, TONE_SET_ALARM, 13); } -int tune_low_bat() +int tune_critical_bat() { - return ioctl(buzzer, TONE_SET_ALARM, 13); + return ioctl(buzzer, TONE_SET_ALARM, 14); } + + void tune_stop() { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -201,9 +203,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -int rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); +} + +void rgbled_set_mode(rgbled_mode_t mode) { + + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); +} + +void rgbled_set_pattern(rgbled_pattern_t *pattern) { - return ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)&color); + if (rgbleds != -1) + ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); } float battery_remaining_estimate_voltage(float voltage) diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index c621b9823..027d0535f 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -45,6 +45,7 @@ #include #include #include +#include bool is_multirotor(const struct vehicle_status_s *current_status); @@ -58,8 +59,8 @@ void tune_positive(void); void tune_neutral(void); void tune_negative(void); int tune_arm(void); -int tune_critical_bat(void); int tune_low_bat(void); +int tune_critical_bat(void); void tune_stop(void); int led_init(void); @@ -68,6 +69,12 @@ int led_toggle(int led); int led_on(int led); int led_off(int led); +void rgbled_set_color(rgbled_color_t color); + +void rgbled_set_mode(rgbled_mode_t mode); + +void rgbled_set_pattern(rgbled_pattern_t *pattern); + /** * Provides a coarse estimate of remaining battery power. * -- cgit v1.2.3 From c543f89ec17048c1b5264623a885a9247a05304c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 23:36:49 +0200 Subject: commander, multirotor_pos_control, multirotor_att_control: bugfixes --- src/modules/commander/commander.cpp | 132 ++++++++++++--------- src/modules/commander/state_machine_helper.cpp | 89 +++++++------- .../multirotor_att_control_main.c | 8 +- .../multirotor_pos_control.c | 22 ++-- src/modules/uORB/topics/vehicle_control_mode.h | 3 +- src/modules/uORB/topics/vehicle_local_position.h | 1 + 6 files changed, 134 insertions(+), 121 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6181dafab..872939d6d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -122,7 +122,7 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -353,27 +353,42 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + case VEHICLE_CMD_NAV_TAKEOFF: { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } break; + } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { + if (is_safe(status, safety, armed)) { - if (((int)(cmd->param1)) == 1) { - /* reboot */ - up_systemreset(); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -519,6 +534,7 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value /* armed topic */ struct actuator_armed_s armed; @@ -760,6 +776,14 @@ int commander_thread_main(int argc, char *argv[]) last_diff_pres_time = diff_pres.timestamp; } + /* Check for valid airspeed/differential pressure measurements */ + if (t - last_diff_pres_time < 2000000 && t > 2000000) { + status.condition_airspeed_valid = true; + + } else { + status.condition_airspeed_valid = false; + } + orb_check(cmd_sub, &updated); if (updated) { @@ -785,6 +809,20 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* set the condition to valid if there has recently been a global position estimate */ + if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { + if (!status.condition_global_position_valid) { + status.condition_global_position_valid = true; + status_changed = true; + } + + } else { + if (status.condition_global_position_valid) { + status.condition_global_position_valid = false; + status_changed = true; + } + } + /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -794,7 +832,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { if (!status.condition_local_position_valid) { status.condition_local_position_valid = true; status_changed = true; @@ -920,36 +958,6 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; - - - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; - - } else { - status.condition_global_position_valid = false; - } - - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; - - } else { - status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } - orb_check(gps_sub, &updated); if (updated) { @@ -1083,6 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { /* DENIED here indicates safety switch not pressed */ @@ -1536,8 +1545,10 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1545,20 +1556,27 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; } else { - /* if not landed: act depending on switches */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2..f313adce4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -109,9 +109,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ - { + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -182,6 +181,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s // 3) Safety switch is present AND engaged -> actuators locked if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { return true; + } else { return false; } @@ -284,6 +284,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; @@ -294,6 +295,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -304,6 +306,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -314,6 +317,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; @@ -324,80 +328,68 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; case NAVIGATION_STATE_AUTO_READY: ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } break; case NAVIGATION_STATE_AUTO_LOITER: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_RTL: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_LAND: @@ -411,6 +403,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 65b19c8e9..1aa24a4fc 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -250,12 +250,12 @@ mc_thread_main(int argc, char *argv[]) /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - if (!control_mode.flag_control_altitude_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* Don't touch throttle in modes with altitude hold, it's handled by position controller. * * Only go to failsafe throttle if last known throttle was @@ -309,12 +309,12 @@ mc_thread_main(int argc, char *argv[]) control_yaw_position = true; } - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0120fa61c..1cb470240 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -221,10 +221,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime home_alt_t = 0; uint64_t local_home_timestamp = 0; - static PID_t xy_pos_pids[2]; - static PID_t xy_vel_pids[2]; - static PID_t z_pos_pid; - static thrust_pid_t z_vel_pid; + PID_t xy_pos_pids[2]; + PID_t xy_vel_pids[2]; + PID_t z_pos_pid; + thrust_pid_t z_vel_pid; thread_running = true; @@ -238,7 +238,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -247,9 +247,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz*/ - if (--paramcheck_counter <= 0) { - paramcheck_counter = 50; + /* check parameters at 1 Hz */ + if (++paramcheck_counter >= 50) { + paramcheck_counter = 0; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -441,14 +441,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subcrive to velocity setpoint if altitude/position control disabled - if (control_mode.flag_control_velocity_enabled) { + if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_altitude_enabled) { + if (control_mode.flag_control_climb_rate_enabled) { thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } - if (control_mode.flag_control_position_enabled) { + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index d83eb45d9..fe169c6e6 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -75,9 +75,10 @@ struct vehicle_control_mode_s //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 76eddeacd..9d3b4468c 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -75,6 +75,7 @@ struct vehicle_local_position_s float home_alt; /**< Altitude in meters LOGME */ float home_hdg; /**< Compass heading in radians -PI..+PI. */ + bool landed; /**< true if vehicle is landed */ }; /** -- cgit v1.2.3 From 2be5240b9f70803417c9648133490409ba40ba55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 Aug 2013 12:37:41 +0200 Subject: commander, multirotor_att_control, position_estimator_inav: position valid flag fixed, other fixes and cleaunup --- src/modules/commander/commander.cpp | 124 +++++++++------------ .../multirotor_att_control_main.c | 20 ++-- .../position_estimator_inav_main.c | 15 ++- 3 files changed, 74 insertions(+), 85 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 872939d6d..d40f6675b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,12 +117,9 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define RC_TIMEOUT 100000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -197,6 +194,8 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); +void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -354,19 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_NAV_TAKEOFF: { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; } - break; - } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { @@ -713,8 +714,6 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { - hrt_abstime t = hrt_absolute_time(); - /* update parameters */ orb_check(param_changed_sub, &updated); @@ -773,16 +772,9 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); orb_check(cmd_sub, &updated); @@ -809,19 +801,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } - /* set the condition to valid if there has recently been a global position estimate */ - if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { - if (!status.condition_global_position_valid) { - status.condition_global_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_global_position_valid) { - status.condition_global_position_valid = false; - status_changed = true; - } - } + /* update condition_global_position_valid */ + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -831,19 +812,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { - if (!status.condition_local_position_valid) { - status.condition_local_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_local_position_valid) { - status.condition_local_position_valid = false; - status_changed = true; - } - } + /* update condition_local_position_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); /* update battery status */ orb_check(battery_sub, &updated); @@ -854,7 +824,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -981,10 +951,9 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + if (!home_position_set && gps_position.fix_type >= 3 && (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk - (t - gps_position.timestamp_position < 2000000) - && !armed.armed) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate home.lat = gps_position.lat; @@ -1019,7 +988,7 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { /* start RC input check */ - if ((t - sp_man.timestamp) < 100000) { + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1123,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { // TODO remove debug code if (!status.rc_signal_cutting_off) { warnx("Reason: not rc_signal_cutting_off\n"); @@ -1136,11 +1105,11 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = t - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { @@ -1157,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO check this /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1213,23 +1182,23 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = t; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; status_changed = true; } @@ -1256,9 +1225,11 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + hrt_abstime t1 = hrt_absolute_time(); + /* publish arming state */ if (arming_state_changed) { - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1266,14 +1237,14 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_changed) { /* publish new navigation state */ control_mode.counter++; - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } /* publish vehicle status at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; - status.timestamp = t; + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); status_changed = false; } @@ -1340,6 +1311,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) { @@ -1367,7 +1350,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp /* position estimated, solid */ led_on(LED_BLUE); - } else if (leds_counter == 0) { + } else if (leds_counter == 6) { /* waiting for position estimate, short blink at 1.25Hz */ led_on(LED_BLUE); @@ -1548,6 +1531,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 1aa24a4fc..5cad667f6 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -367,14 +367,20 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - float gyro[3]; - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators); + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); /* update state */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0530c2dea..0e7e0ef5d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,6 +75,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -490,7 +491,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; if (can_estimate_pos) { /* inertial filter prediction for position */ @@ -501,14 +502,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); - } + if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } -- cgit v1.2.3 From 6ff3f51f88c2335f225a2a391de5ee353487a69b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 15:47:42 +0200 Subject: Added dim RGB led support, not operational yet --- src/modules/commander/commander.cpp | 35 +++++++++++++++++++++++++---------- 1 file changed, 25 insertions(+), 10 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2e5d080b9..30906034e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -153,6 +154,8 @@ static unsigned int failsafe_lowlevel_timeout_ms; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; +/* if connected via USB */ +static bool on_usb_power = false; /* tasks waiting for low prio thread */ @@ -205,6 +208,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st void print_reject_mode(const char *msg); +void print_status(); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); /** @@ -244,6 +249,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("\tcommander is running\n"); + print_status(); } else { warnx("\tcommander not started\n"); @@ -265,6 +271,10 @@ void usage(const char *reason) exit(1); } +void print_status() { + warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); +} + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -865,9 +875,14 @@ int commander_thread_main(int argc, char *argv[]) status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; + + /* check if board is connected via USB */ + struct stat statbuf; + //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } - warnx("bat remaining: %2.2f", status.battery_remaining); + // XXX remove later + //warnx("bat remaining: %2.2f", status.battery_remaining); /* if battery voltage is getting lower, warn using buzzer, etc. */ if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) { @@ -1362,22 +1377,22 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { /* armed, solid */ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[0] = RGBLED_COLOR_AMBER; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[0] = RGBLED_COLOR_RED; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { - pattern.color[0] = RGBLED_COLOR_GREEN; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN; } pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { for (i=0; i<3; i++) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i*2] = RGBLED_COLOR_AMBER; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { - pattern.color[i*2] = RGBLED_COLOR_GREEN; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } pattern.duration[i*2] = 200; @@ -1385,13 +1400,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang pattern.duration[i*2+1] = 800; } if (status->condition_global_position_valid) { - pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; pattern.duration[i*2] = 1000; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 800; } else { for (i=3; i<6; i++) { - pattern.color[i*2] = RGBLED_COLOR_BLUE; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; pattern.duration[i*2] = 100; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 100; @@ -1402,7 +1417,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else { for (i=0; i<3; i++) { - pattern.color[i*2] = RGBLED_COLOR_RED; + pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; pattern.duration[i*2] = 200; pattern.color[i*2+1] = RGBLED_COLOR_OFF; pattern.duration[i*2+1] = 200; -- cgit v1.2.3 From 36d474bfa31a44c49647cf8fc9d825cb1e919182 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 17 Aug 2013 18:39:46 +0200 Subject: WIP on getting low-priority non-command tasks out of the commander main loop --- src/modules/commander/commander.cpp | 288 +++++++++++++++++++----------------- 1 file changed, 149 insertions(+), 139 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034e..891dd893b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -280,6 +280,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + /* only handle high-priority commands here */ + /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { @@ -363,95 +365,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: + // XXX implement later + result = VEHICLE_CMD_RESULT_DENIED; break; - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { - - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ - - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - break; - - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if ((int)(cmd->param1) == 1) { - /* gyro calibration */ - new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - - } else if ((int)(cmd->param2) == 1) { - /* magnetometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - - } else if ((int)(cmd->param3) == 1) { - /* zero-altitude pressure calibration */ - //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - } else if ((int)(cmd->param4) == 1) { - /* RC calibration */ - new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - - } else if ((int)(cmd->param5) == 1) { - /* accelerometer calibration */ - new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - - } else if ((int)(cmd->param6) == 1) { - /* airspeed calibration */ - new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - - if (((int)(cmd->param1)) == 0) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - - } else if (((int)(cmd->param1)) == 1) { - new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - } - - /* check if we have new task and no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - low_prio_task = new_low_prio_task; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - default: break; } @@ -460,6 +377,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (result == VEHICLE_CMD_RESULT_ACCEPTED) { tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* we do not care in the high prio loop about commands we don't know */ } else { tune_negative(); @@ -472,19 +391,24 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); } } /* send any requested ACKs */ - if (cmd->confirmation > 0) { + if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } } +static struct vehicle_status_s status; + +/* armed topic */ +static struct actuator_armed_s armed; + +static struct safety_s safety; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -524,13 +448,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); /* armed topic */ - struct actuator_armed_s armed; orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); @@ -631,7 +553,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); - struct safety_s safety; memset(&safety, 0, sizeof(safety)); safety.safety_switch_available = false; safety.safety_off = false; @@ -1633,80 +1554,169 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); - low_prio_task = LOW_PRIO_TASK_NONE; + /* Subscribe to command topic */ + int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); + struct vehicle_command_s cmd; + memset(&cmd, 0, sizeof(cmd)); + + /* wakeup source(s) */ + struct pollfd fds[1]; + + /* use the gyro to pace output - XXX BROKEN if we are using the L3GD20 */ + fds[0].fd = cmd_sub; + fds[0].events = POLLIN; while (!thread_should_exit) { - switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + /* wait for up to 100ms for data */ + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + /* timed out - periodic check for _task_should_exit, etc. */ + if (pret == 0) + continue; - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - } + /* this is undesirable but not much we can do - might want to flag unhappy status */ + if (pret < 0) { + warn("poll error %d, %d", pret, errno); + continue; + } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + /* if we reach here, we have a valid command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - case LOW_PRIO_TASK_PARAM_SAVE: + /* ignore commands the high-prio loop handles */ + if (cmd.command == VEHICLE_CMD_DO_SET_MODE || + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + continue; - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + /* result of the command */ + uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - } + /* only handle low-priority commands here */ + switch (cmd.command) { - low_prio_task = LOW_PRIO_TASK_NONE; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + /* reboot to bootloader */ + systemreset(true); + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } break; - case LOW_PRIO_TASK_GYRO_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - do_gyro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + /* try to go to INIT/PREFLIGHT arming state */ - case LOW_PRIO_TASK_MAG_CALIBRATION: + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + result = VEHICLE_CMD_RESULT_DENIED; + break; + } - do_mag_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + do_gyro_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + do_mag_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - // do_baro_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + result = VEHICLE_CMD_RESULT_DENIED; + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + result = VEHICLE_CMD_RESULT_DENIED; - case LOW_PRIO_TASK_RC_CALIBRATION: + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + do_accel_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; - // do_rc_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + do_airspeed_calibration(mavlink_fd); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } - case LOW_PRIO_TASK_ACCEL_CALIBRATION: + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - do_accel_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + break; + } - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_STORAGE: { - do_airspeed_calibration(mavlink_fd); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + tune_error(); + result = VEHICLE_CMD_RESULT_FAILED; + } + } + + break; + } - case LOW_PRIO_TASK_NONE: default: - /* slow down to 10Hz */ - usleep(100000); break; } + /* supported command handling stop */ + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + + } else { + tune_negative(); + + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + } + } + + /* send any requested ACKs */ + if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + /* send acknowledge command */ + // XXX TODO + } + } return 0; -- cgit v1.2.3 From e5f1a7c2c3a67648829ec0dff5bf290dddc25847 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 12:42:24 +0200 Subject: Better transparency in IO mode display, fixes to commander arming, minimum publishing rate for arming --- src/drivers/px4io/px4io.cpp | 4 +- src/modules/commander/commander.cpp | 51 +++++++++++++++++++++++--- src/modules/commander/state_machine_helper.cpp | 2 +- 3 files changed, 49 insertions(+), 8 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8318238e2..0f90db858 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1519,8 +1519,8 @@ PX4IO::print_status() uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); printf("arming 0x%04x%s%s%s%s%s%s\n", arming, - ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), - ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : " FMU_DISARMED"), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : " IO_ARM_DENIED"), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 30906034e..12543800e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -273,6 +273,43 @@ void usage(const char *reason) void print_status() { warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); + + /* read all relevant states */ + int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + struct vehicle_status_s state; + orb_copy(ORB_ID(vehicle_status), state_sub, &state); + + const char* armed_str; + + switch (state.arming_state) { + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + default: + armed_str = "ERR: UNKNOWN STATE"; + break; + } + + + warnx("arming: %s", armed_str); } void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) @@ -945,9 +982,9 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; + // bool global_pos_valid = status.condition_global_position_valid; + // bool local_pos_valid = status.condition_local_position_valid; + // bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ @@ -1274,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } - /* publish vehicle status at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ + if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; status.timestamp = t; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* play arming and battery warning tunes */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5842a33b1..1e31573d6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -119,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * { ret = TRANSITION_CHANGED; armed->armed = true; - armed->ready_to_arm = false; + armed->ready_to_arm = true; } break; -- cgit v1.2.3 From ffc2a8b7a358a2003e5ed548c41878b33e7aa726 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 18 Aug 2013 23:05:26 +0200 Subject: vehicle_local_position topic updated, position_estimator_inav and commander fixes, only altitude estimate required for SETBELT now. --- src/modules/commander/commander.cpp | 96 +++++----- src/modules/commander/state_machine_helper.cpp | 27 +-- .../multirotor_pos_control.c | 20 +- .../position_estimator_inav_main.c | 201 +++++++++++---------- .../position_estimator_inav_params.c | 3 - .../position_estimator_inav_params.h | 2 - src/modules/sdlog2/sdlog2.c | 7 +- src/modules/sdlog2/sdlog2_messages.h | 9 +- src/modules/uORB/topics/vehicle_local_position.h | 39 ++-- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 195 insertions(+), 213 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d40f6675b..ab7d2e6cf 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -551,7 +551,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&control_mode, 0, sizeof(control_mode)); status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_STANDBY; + status.navigation_state = NAVIGATION_STATE_DIRECT; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -812,8 +812,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* update condition_local_position_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); + /* update condition_local_position_valid and condition_local_altitude_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); /* update battery status */ orb_check(battery_sub, &updated); @@ -1512,68 +1513,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v { transition_result_t res = TRANSITION_DENIED; - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - /* ARMED */ - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); + break; - case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + case MAIN_STATE_AUTO: + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + + } else { + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } - - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - - break; - - default: - break; } - } else { - /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); + break; + + default: + break; } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f313adce4..76c7eaf92 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -217,9 +217,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: - /* need position estimate */ - // TODO only need altitude estimate really - if (current_state->condition_local_position_valid) { + /* need altitude estimate */ + if (current_state->condition_local_altitude_valid) { ret = TRANSITION_CHANGED; } @@ -227,7 +226,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: - /* need position estimate */ + /* need local position estimate */ if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -236,8 +235,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: - /* need position estimate */ - if (current_state->condition_local_position_valid) { + /* need global position estimate */ + if (current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -277,17 +276,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } else { switch (new_navigation_state) { - case NAVIGATION_STATE_STANDBY: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = false; - break; - case NAVIGATION_STATE_DIRECT: ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; @@ -394,9 +382,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ case NAVIGATION_STATE_AUTO_LAND: - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + /* deny transitions from landed state */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1cb470240..80ce33cda 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -219,7 +219,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; - uint64_t local_home_timestamp = 0; + uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; @@ -316,11 +316,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, project global setpoints to local frame */ //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.home_timestamp != local_home_timestamp) { - local_home_timestamp = local_pos.home_timestamp; + if (local_pos.ref_timestamp != local_ref_timestamp) { + local_ref_timestamp = local_pos.ref_timestamp; /* init local projection using local position home */ - double lat_home = local_pos.home_lat * 1e-7; - double lon_home = local_pos.home_lon * 1e-7; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); @@ -338,7 +338,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = -global_pos_sp.altitude; } else { - local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -355,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.home_timestamp != home_alt_t) { + if (local_pos.ref_timestamp != home_alt_t) { if (home_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.home_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - home_alt; } - home_alt_t = local_pos.home_timestamp; - home_alt = local_pos.home_alt; + home_alt_t = local_pos.ref_timestamp; + home_alt = local_pos.ref_alt; } if (control_mode.flag_control_altitude_enabled) { diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0e7e0ef5d..81f938719 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -76,6 +76,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -169,10 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0f; //[°] --> 47.0 - double lon_current = 0.0f; //[°] --> 8.5 - double alt_current = 0.0f; //[m] above MSL - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -216,21 +213,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - struct pollfd fds_init[2] = { + struct pollfd fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ - bool wait_gps = params.use_gps; + /* wait for initial baro value */ bool wait_baro = true; - hrt_abstime wait_gps_start = 0; - const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix thread_running = true; - while ((wait_gps || wait_baro) && !thread_should_exit) { - int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); + while (wait_baro && !thread_should_exit) { + int ret = poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ @@ -253,43 +246,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 /= (float) baro_init_cnt; warnx("init baro: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); - } - } - } - - if (params.use_gps && (fds_init[1].revents & POLLIN)) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - - if (wait_gps && gps.fix_type >= 3) { - hrt_abstime t = hrt_absolute_time(); - - if (wait_gps_start == 0) { - wait_gps_start = t; - - } else if (t - wait_gps_start > wait_gps_delay) { - wait_gps = false; - /* get GPS position for first initialization */ - lat_current = gps.lat * 1e-7; - lon_current = gps.lon * 1e-7; - alt_current = gps.alt * 1e-3; - - local_pos.home_lat = lat_current * 1e7; - local_pos.home_lon = lon_current * 1e7; - local_pos.home_hdg = 0.0f; - local_pos.home_timestamp = t; - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.z_valid = true; + local_pos.v_z_valid = true; + local_pos.global_z = true; } } } } } + bool ref_xy_inited = false; + hrt_abstime ref_xy_init_start = 0; + const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix + hrt_abstime t_prev = 0; uint16_t accel_updates = 0; @@ -337,7 +308,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -428,8 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 += sonar_corr; warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; @@ -444,29 +415,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - if (params.use_gps && (fds[5].revents & POLLIN)) { + if (fds[5].revents & POLLIN) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + /* initialize reference position if needed */ + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + hrt_abstime t = hrt_absolute_time(); + + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } } gps_updates++; } else { + /* no GPS lock */ memset(gps_corr, 0, sizeof(gps_corr)); } } @@ -490,10 +489,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; + bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; + bool flow_valid = false; // TODO implement opt flow + + /* try to estimate xy even if no absolute position source available, + * if using optical flow velocity will be correct in this case */ + bool can_estimate_xy = gps_valid || flow_valid; - if (can_estimate_pos) { + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); @@ -502,12 +505,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - - if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + if (gps_valid) { + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + } } } @@ -533,43 +537,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; + /* publish local position */ local_pos.timestamp = t; - local_pos.valid = can_estimate_pos; + local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.v_xy_valid = can_estimate_xy; + local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.absolute_alt = local_pos.home_alt - local_pos.z; - local_pos.hdg = att.yaw; - - if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) - && (isfinite(local_pos.y)) - && (isfinite(local_pos.vy)) - && (isfinite(local_pos.z)) - && (isfinite(local_pos.vz))) { - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - - if (params.use_gps) { - double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - - global_pos.valid = local_pos.valid; - global_pos.timestamp = t; - global_pos.time_gps_usec = gps.time_gps_usec; - global_pos.lat = (int32_t)(est_lat * 1e7); - global_pos.lon = (int32_t)(est_lon * 1e7); - global_pos.alt = local_pos.home_alt - local_pos.z; - global_pos.relative_alt = -local_pos.z; - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - global_pos.vz = local_pos.vz; - global_pos.hdg = local_pos.hdg; - - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); - } + local_pos.landed = false; // TODO + + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + + /* publish global position */ + global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.time_gps_usec = gps.time_gps_usec; + } + /* set valid values even if position is not valid */ + if (local_pos.v_xy_valid) { + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + } + if (local_pos.z_valid) { + global_pos.relative_alt = -local_pos.z; } + if (local_pos.global_z) { + global_pos.alt = local_pos.ref_alt - local_pos.z; + } + if (local_pos.v_z_valid) { + global_pos.vz = local_pos.vz; + } + global_pos.timestamp = t; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 70248b3b7..801e20781 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,6 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); @@ -55,7 +54,6 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->use_gps = param_find("INAV_USE_GPS"); h->w_alt_baro = param_find("INAV_W_ALT_BARO"); h->w_alt_acc = param_find("INAV_W_ALT_ACC"); h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); @@ -73,7 +71,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->use_gps, &(p->use_gps)); param_get(h->w_alt_baro, &(p->w_alt_baro)); param_get(h->w_alt_acc, &(p->w_alt_acc)); param_get(h->w_alt_sonar, &(p->w_alt_sonar)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 1e70a3c48..ed6f61e04 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,7 +41,6 @@ #include struct position_estimator_inav_params { - int use_gps; float w_alt_baro; float w_alt_acc; float w_alt_sonar; @@ -56,7 +55,6 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t use_gps; param_t w_alt_baro; param_t w_alt_acc; param_t w_alt_sonar; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 30dc7df9e..7f8648d95 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1042,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; - log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; - log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; - log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 38e2596b2..dd98f65a9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -106,10 +106,9 @@ struct log_LPOS_s { float vx; float vy; float vz; - float hdg; - int32_t home_lat; - int32_t home_lon; - float home_alt; + int32_t ref_lat; + int32_t ref_lon; + float ref_alt; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -260,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 9d3b4468c..26e8f335b 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -54,27 +54,26 @@ */ struct vehicle_local_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - float x; /**< X positin in meters in NED earth-fixed frame */ - float y; /**< X positin in meters in NED earth-fixed frame */ - float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - - // TODO Add covariances here - + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool xy_valid; /**< true if x and y are valid */ + bool z_valid; /**< true if z is valid */ + bool v_xy_valid; /**< true if vy and vy are valid */ + bool v_z_valid; /**< true if vz is valid */ + /* Position in local NED frame */ + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< X position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + /* Velocity in NED frame */ + float vx; /**< Ground X Speed (Latitude), m/s in NED */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED */ /* Reference position in GPS / WGS84 frame */ - uint64_t home_timestamp;/**< Time when home position was set */ - int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ - float home_alt; /**< Altitude in meters LOGME */ - float home_hdg; /**< Compass heading in radians -PI..+PI. */ - + bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ + uint64_t ref_timestamp; /**< Time when reference position was set */ + int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ + int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6690fb2be..4317e07f2 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -68,8 +68,7 @@ typedef enum { /* navigation state machine */ typedef enum { - NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed - NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization NAVIGATION_STATE_STABILIZE, // attitude stabilization NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization @@ -203,6 +202,7 @@ struct vehicle_status_s bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; + bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ -- cgit v1.2.3 From 3370ceceaf706dda0856888b09c1086e8bf79c8d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 08:43:16 +0200 Subject: vehicle_control_mode.flag_armed added, reset integrals in multirotor_att_control when disarmed --- src/modules/commander/commander.cpp | 3 +- .../multirotor_att_control_main.c | 33 +++++++++------------- .../multirotor_attitude_control.c | 9 +++--- .../multirotor_attitude_control.h | 2 +- .../multirotor_rate_control.c | 7 +++-- .../multirotor_rate_control.h | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 2 ++ 7 files changed, 29 insertions(+), 29 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ab7d2e6cf..7d74e0cfe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1235,8 +1235,9 @@ int commander_thread_main(int argc, char *argv[]) } /* publish control mode */ - if (navigation_state_changed) { + if (navigation_state_changed || arming_state_changed) { /* publish new navigation state */ + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic control_mode.counter++; control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 5cad667f6..c057ef364 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -142,16 +142,13 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); /* welcome user */ - printf("[multirotor_att_control] starting\n"); + warnx("starting"); /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; - - /* store if we stopped a yaw movement */ bool reset_yaw_sp = true; /* prepare the handle for the failsafe throttle */ @@ -211,8 +208,7 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /** STEP 1: Define which input is the dominating control input */ + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { @@ -220,7 +216,6 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; -// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -229,13 +224,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; -// printf("thrust_att=%8.4f\n",offboard_sp.p4); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - } else if (control_mode.flag_control_manual_enabled) { /* direct manual input */ if (control_mode.flag_control_attitude_enabled) { @@ -265,7 +258,7 @@ mc_thread_main(int argc, char *argv[]) * multicopter (throttle = 0) does not make it jump up in the air * if shutting down his remote. */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.thrust = failsafe_throttle; @@ -305,7 +298,6 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; reset_yaw_sp = false; } - control_yaw_position = true; } @@ -347,22 +339,25 @@ mc_thread_main(int argc, char *argv[]) } } - /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); + /* run rates controller if needed */ if (control_mode.flag_control_rates_enabled) { /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + bool rates_sp_updated = false; + orb_check(rates_sp_sub, &rates_sp_updated); - if (rates_sp_valid) { + if (rates_sp_updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); } @@ -371,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) rates[0] = att.rollspeed; rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators); + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; @@ -391,7 +386,7 @@ mc_thread_main(int argc, char *argv[]) } /* end of poll return value check */ } - printf("[multirotor att control] stopping, disarming motors.\n"); + warnx("stopping, disarming motors"); /* kill all outputs */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8f19c6a4b..12d16f7c7 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } - /* reset integral if on ground */ - if (att_sp->thrust < 0.1f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); + //TODO pid_reset_integral(&yaw_controller); } - /* calculate current control outputs */ /* control pitch (forward) output */ @@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (control_yaw_position) { /* control yaw rate */ + // TODO use pid lib /* positive error: rotate to right, negative error, rotate to left (NED frame) */ // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index e78f45c47..431a435f7 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -60,6 +60,6 @@ #include void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e58d357d5..0a336be47 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, bool reset_integral) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* reset integral if on ground */ - if (rate_sp->thrust < 0.01f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); + // TODO pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 362b5ed86..ca7794c59 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -59,6 +59,6 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, bool reset_integral); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index fe169c6e6..67aeac372 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -64,6 +64,8 @@ struct vehicle_control_mode_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + bool flag_armed; + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ // XXX needs yet to be set by state machine helper -- cgit v1.2.3 From 9610f7a0d7ba7abf7d88c4b3096285e3da68e04d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 09:53:00 +0200 Subject: Fixed merge compile errors --- src/modules/commander/commander.cpp | 103 +++++++++++++++--------------------- 1 file changed, 42 insertions(+), 61 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0f145e1eb..daab7e436 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -418,23 +418,14 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { -<<<<<<< HEAD - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ -======= if (((int)(cmd->param1)) == 1) { /* reboot */ - up_systemreset(); + systemreset(false); } else if (((int)(cmd->param1)) == 3) { /* reboot to bootloader */ - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 + systemreset(true); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -768,13 +759,7 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { -<<<<<<< HEAD - hrt_abstime t = hrt_absolute_time(); - status_changed = false; - -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* update parameters */ orb_check(param_changed_sub, &updated); @@ -882,17 +867,11 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); -<<<<<<< HEAD - - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { -======= - warnx("bat v: %2.2f", battery.voltage_v); + // warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -1010,39 +989,39 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ -<<<<<<< HEAD - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - // bool global_pos_valid = status.condition_global_position_valid; - // bool local_pos_valid = status.condition_local_position_valid; - // bool airspeed_valid = status.condition_airspeed_valid; +// <<<<<<< HEAD +// /* store current state to reason later about a state change */ +// // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; +// // bool global_pos_valid = status.condition_global_position_valid; +// // bool local_pos_valid = status.condition_local_position_valid; +// // bool airspeed_valid = status.condition_airspeed_valid; - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; +// /* check for global or local position updates, set a timeout of 2s */ +// if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { +// status.condition_global_position_valid = true; - } else { - status.condition_global_position_valid = false; - } +// } else { +// status.condition_global_position_valid = false; +// } - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; +// if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { +// status.condition_local_position_valid = true; - } else { - status.condition_local_position_valid = false; - } +// } else { +// status.condition_local_position_valid = false; +// } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; +// /* Check for valid airspeed/differential pressure measurements */ +// if (t - last_diff_pres_time < 2000000 && t > 2000000) { +// status.condition_airspeed_valid = true; - } else { - status.condition_airspeed_valid = false; - } +// } else { +// status.condition_airspeed_valid = false; +// } -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 +// ======= +// >>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 orb_check(gps_sub, &updated); if (updated) { @@ -1362,9 +1341,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1433,6 +1412,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { @@ -1512,18 +1503,8 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* not ready to arm, blink at 10Hz */ } -<<<<<<< HEAD rgbled_set_pattern(&pattern); } -======= - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 6) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { -- cgit v1.2.3 From a9743f04be59aee7bb96a5bb99ae8d8155eb19de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:09:06 +0200 Subject: Fixed status changed flag --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daab7e436..98aab8788 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1317,6 +1317,8 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { + status_changed = false; } hrt_abstime t1 = hrt_absolute_time(); @@ -1446,8 +1448,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - warnx("changed"); - int i; rgbled_pattern_t pattern; memset(&pattern, 0, sizeof(pattern)); -- cgit v1.2.3 From 449dc78ae69e888d986185f120aa8c6549ef5c2b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 18:33:04 +0200 Subject: commander, multirotor_pos_control, sdlog2: bugfixes --- src/modules/commander/commander.cpp | 17 +- .../multirotor_pos_control.c | 249 ++++++++++++++------- .../multirotor_pos_control_params.c | 7 +- .../multirotor_pos_control_params.h | 11 +- .../position_estimator_inav_main.c | 7 + src/modules/sdlog2/sdlog2_messages.h | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 6 +- 7 files changed, 197 insertions(+), 102 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7d74e0cfe..50acec7e0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -353,16 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_NAV_TAKEOFF: { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (armed->armed) { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } else { + /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b2f6b33e3..0d5a537ea 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -1,7 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,14 +211,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - bool reset_sp_alt = true; - bool reset_sp_pos = true; + bool global_pos_sp_reproject = false; + bool global_pos_sp_valid = false; + bool local_pos_sp_valid = false; + bool reset_sp_z = true; + bool reset_sp_xy = true; + bool reset_int_z = true; + bool reset_int_z_manual = false; + bool reset_int_xy = true; + bool was_armed = false; + bool reset_integral = true; + hrt_abstime t_prev = 0; /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - float home_alt = 0.0f; - hrt_abstime home_alt_t = 0; + float ref_alt = 0.0f; + hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; @@ -242,11 +251,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; - bool global_pos_sp_updated = false; while (!thread_should_exit) { - orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz */ if (++paramcheck_counter >= 50) { paramcheck_counter = 0; @@ -260,11 +266,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); /* use integral_limit_out = tilt_max / 2 */ float i_limit; + if (params.xy_vel_i == 0.0) { i_limit = params.tilt_max / params.xy_vel_i / 2.0; + } else { i_limit = 1.0f; // not used really } + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } @@ -273,9 +282,20 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* only check global position setpoint updates but not read */ - if (!global_pos_sp_updated) { - orb_check(global_pos_sp_sub, &global_pos_sp_updated); + bool updated; + + orb_check(control_mode_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); + } + + orb_check(global_pos_sp_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + global_pos_sp_valid = true; + global_pos_sp_reproject = true; } hrt_abstime t = hrt_absolute_time(); @@ -288,6 +308,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) dt = 0.0f; } + if (control_mode.flag_armed && !was_armed) { + /* reset setpoints and integrals on arming */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + } + + was_armed = control_mode.flag_armed; + t_prev = t; if (control_mode.flag_control_altitude_enabled || control_mode.flag_control_velocity_enabled || control_mode.flag_control_position_enabled) { @@ -296,77 +326,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (control_mode.flag_control_manual_enabled) { - /* manual mode, reset setpoints if needed */ - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside - mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - } - - if (control_mode.flag_control_position_enabled && reset_sp_pos) { - reset_sp_pos = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - pid_reset_integral(&xy_vel_pids[0]); - pid_reset_integral(&xy_vel_pids[1]); - mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - } - } else { - /* non-manual mode, project global setpoints to local frame */ - //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.ref_timestamp != local_ref_timestamp) { - local_ref_timestamp = local_pos.ref_timestamp; - /* init local projection using local position home */ - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); - mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_updated) { - global_pos_sp_updated = false; - orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - } - } - float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; - /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.ref_timestamp != home_alt_t) { - if (home_alt_t != 0) { + /* manual control */ + /* check for reference point updates and correct setpoint */ + if (local_pos.ref_timestamp != ref_alt_t) { + if (ref_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - ref_alt; } - home_alt_t = local_pos.ref_timestamp; - home_alt = local_pos.ref_alt; + ref_alt_t = local_pos.ref_timestamp; + ref_alt = local_pos.ref_alt; + // TODO also correct XY setpoint } + /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { - /* move altitude setpoint with manual controls */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z); + } + + /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { @@ -383,7 +369,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } if (control_mode.flag_control_position_enabled) { - /* move position setpoint with manual controls */ + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* move position setpoint with roll/pitch stick */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); @@ -410,12 +405,68 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + + /* local position setpoint is valid and can be used for loiter after position controlled mode */ + local_pos_sp_valid = control_mode.flag_control_position_enabled; + + /* force reprojection of global setpoint after manual mode */ + global_pos_sp_reproject = true; + + } else { + /* non-manual mode, use global setpoint */ + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + } + + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } + + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + + } else { + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } + + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + } + + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } } /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; + } else { + reset_sp_z = true; global_vel_sp.vz = 0.0f; } @@ -426,13 +477,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (xy_vel_sp_norm > 1.0f) { global_vel_sp.vx /= xy_vel_sp_norm; global_vel_sp.vy /= xy_vel_sp_norm; } } else { - reset_sp_pos = true; + reset_sp_xy = true; global_vel_sp.vx = 0.0f; global_vel_sp.vy = 0.0f; } @@ -444,20 +496,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - bool reset_integral = !control_mode.flag_armed; + if (control_mode.flag_control_climb_rate_enabled) { - if (reset_integral) { - thrust_pid_set_integral(&z_vel_pid, params.thr_min); + if (reset_int_z) { + reset_int_z = false; + float i = params.thr_min; + + if (reset_int_z_manual) { + i = manual.throttle; + + if (i < params.thr_min) { + i = params.thr_min; + + } else if (i > params.thr_max) { + i = params.thr_max; + } + } + + thrust_pid_set_integral(&z_vel_pid, -i); + mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i); } + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; + + } else { + /* reset thrust integral when altitude control enabled */ + reset_int_z = true; } + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ - if (reset_integral) { + if (reset_int_xy) { + reset_int_xy = false; pid_reset_integral(&xy_vel_pids[0]); pid_reset_integral(&xy_vel_pids[1]); + mavlink_log_info(mavlink_fd, "[mpc] reset pos integral"); } + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); @@ -471,11 +547,15 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (tilt > params.tilt_max) { tilt = params.tilt_max; } + /* convert direction to body frame */ thrust_xy_dir -= att.yaw; /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * tilt; att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); + + } else { + reset_int_xy = true; } att_sp.timestamp = hrt_absolute_time(); @@ -483,14 +563,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } + } else { - reset_sp_alt = true; - reset_sp_pos = true; + /* position controller disabled, reset setpoints */ + reset_sp_z = true; + reset_sp_xy = true; + reset_int_z = true; + reset_int_xy = true; + global_pos_sp_reproject = true; } + /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ + reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; + /* run at approximately 50 Hz */ usleep(20000); - } warnx("stopped"); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 1094fd23b..0b09c9ea7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +34,7 @@ /* * @file multirotor_pos_control_params.c - * + * * Parameters for multirotor_pos_control */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 14a3de2e5..3ec85a364 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +33,9 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.h - * - * Parameters for position controller + * @file multirotor_pos_control_params.h + * + * Parameters for multirotor_pos_control */ #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81f938719..c0cfac880 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -508,6 +508,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); @@ -554,6 +555,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* publish global position */ global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); @@ -561,21 +563,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.time_gps_usec = gps.time_gps_usec; } + /* set valid values even if position is not valid */ if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); } + if (local_pos.z_valid) { global_pos.relative_alt = -local_pos.z; } + if (local_pos.global_z) { global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.timestamp = t; orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index dd98f65a9..d99892fe2 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -259,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), + LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 67aeac372..4f4db5dbc 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,7 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; @@ -73,11 +73,9 @@ struct vehicle_control_mode_s bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // XXX shouldn't be necessairy - //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ -- cgit v1.2.3 From db950f74893a108302a167729a91765269981e7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 Aug 2013 12:17:15 +0200 Subject: position_estimator_inav: "landed" detector implemented, bugfixes --- src/modules/commander/commander.cpp | 11 ++ .../multirotor_pos_control.c | 4 + .../multirotor_pos_control_params.c | 4 +- .../position_estimator_inav_main.c | 128 ++++++++++++++------- .../position_estimator_inav_params.c | 11 +- .../position_estimator_inav_params.h | 6 + 6 files changed, 118 insertions(+), 46 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50acec7e0..04e6dd2cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -820,6 +820,17 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + if (status.condition_local_altitude_valid) { + if (status.condition_landed != local_position.landed) { + status.condition_landed = local_position.landed; + status_changed = true; + if (status.condition_landed) { + mavlink_log_info(mavlink_fd, "[cmd] LANDED"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] IN AIR"); + } + } + } /* update battery status */ orb_check(battery_sub, &updated); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0d5a537ea..a6ebeeacb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -459,6 +459,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish local position setpoint as projection of global position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + + /* reset setpoints after non-manual modes */ + reset_sp_xy = true; + reset_sp_z = true; } /* run position & altitude controllers, calculate velocity setpoint */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 0b09c9ea7..9c1ef2edb 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -41,8 +41,8 @@ #include "multirotor_pos_control_params.h" /* controller parameters */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); -PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.8f); PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index c0cfac880..3466841c4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -52,10 +52,11 @@ #include #include #include -#include #include +#include +#include +#include #include -#include #include #include #include @@ -169,14 +170,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_cnt = 0; int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ + float alt_avg = 0.0f; + bool landed = true; + hrt_abstime landed_time = 0; uint32_t accel_counter = 0; uint32_t baro_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - /* make sure that baroINITdone = false */ + struct actuator_controls_effective_s actuator; + memset(&actuator, 0, sizeof(actuator)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; @@ -192,7 +197,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); @@ -294,9 +300,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime sonar_time = 0; /* main loop */ - struct pollfd fds[6] = { + struct pollfd fds[7] = { { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = actuator_sub, .events = POLLIN }, + { .fd = armed_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = optical_flow_sub, .events = POLLIN }, @@ -308,7 +315,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 7, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -328,20 +335,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) parameters_update(&pos_inav_param_handles, ¶ms); } - /* vehicle status */ + /* actuator */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, - &vehicle_status); + orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator); } - /* vehicle attitude */ + /* armed */ if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + } + + /* vehicle attitude */ + if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; } /* sensor combined */ - if (fds[3].revents & POLLIN) { + if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter > accel_counter) { @@ -378,7 +389,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* optical flow */ - if (fds[4].revents & POLLIN) { + if (fds[5].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { @@ -415,33 +426,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - if (fds[5].revents & POLLIN) { - /* vehicle GPS position */ + /* vehicle GPS position */ + if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { /* initialize reference position if needed */ - if (ref_xy_inited) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - /* calculate correction for position */ - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - - /* calculate correction for velocity */ - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; - - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; - } - - } else { - hrt_abstime t = hrt_absolute_time(); - + if (!ref_xy_inited) { if (ref_xy_init_start == 0) { ref_xy_init_start = t; @@ -462,12 +453,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - gps_updates++; + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + } } else { /* no GPS lock */ memset(gps_corr, 0, sizeof(gps_corr)); + ref_xy_init_start = 0; } + + gps_updates++; } } @@ -516,9 +527,40 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + /* detect land */ + alt_avg += (z_est[0] - alt_avg) * dt / params.land_t; + float alt_disp = z_est[0] - alt_avg; + alt_disp = alt_disp * alt_disp; + float land_disp2 = params.land_disp * params.land_disp; + /* get actual thrust output */ + float thrust = armed.armed ? actuator.control_effective[3] : 0.0f; + + if (landed) { + if (alt_disp > land_disp2 && thrust > params.land_thr) { + landed = false; + landed_time = 0; + } + + } else { + if (alt_disp < land_disp2 && thrust < params.land_thr) { + if (landed_time == 0) { + landed_time = t; // land detected first time + + } else { + if (t > landed_time + params.land_t * 1000000.0f) { + landed = true; + landed_time = 0; + } + } + + } else { + landed_time = 0; + } + } + if (verbose_mode) { /* print updates rate */ - if (t - updates_counter_start > updates_counter_len) { + if (t > updates_counter_start + updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; warnx( "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", @@ -536,7 +578,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (t - pub_last > pub_interval) { + if (t > pub_last + pub_interval) { pub_last = t; /* publish local position */ local_pos.timestamp = t; @@ -549,7 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.landed = false; // TODO + local_pos.landed = landed; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 801e20781..4f9ddd009 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,7 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); @@ -51,6 +51,9 @@ PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); +PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); +PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -65,6 +68,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->flow_k = param_find("INAV_FLOW_K"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); + h->land_t = param_find("INAV_LAND_T"); + h->land_disp = param_find("INAV_LAND_DISP"); + h->land_thr = param_find("INAV_LAND_THR"); return OK; } @@ -82,6 +88,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->flow_k, &(p->flow_k)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->land_t, &(p->land_t)); + param_get(h->land_disp, &(p->land_disp)); + param_get(h->land_thr, &(p->land_thr)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index ed6f61e04..61570aea7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -52,6 +52,9 @@ struct position_estimator_inav_params { float flow_k; float sonar_filt; float sonar_err; + float land_t; + float land_disp; + float land_thr; }; struct position_estimator_inav_param_handles { @@ -66,6 +69,9 @@ struct position_estimator_inav_param_handles { param_t flow_k; param_t sonar_filt; param_t sonar_err; + param_t land_t; + param_t land_disp; + param_t land_thr; }; /** -- cgit v1.2.3 From 5f1004117f8086c4bba5b4031f3aebd73411682c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 22 Aug 2013 15:57:17 +0200 Subject: Restore proper feedback (mavlink and tone) for calibration commands, etc --- .../commander/accelerometer_calibration.cpp | 6 +- src/modules/commander/accelerometer_calibration.h | 2 +- src/modules/commander/airspeed_calibration.cpp | 18 +- src/modules/commander/airspeed_calibration.h | 4 +- src/modules/commander/baro_calibration.cpp | 10 +- src/modules/commander/baro_calibration.h | 4 +- src/modules/commander/commander.cpp | 206 +++++++++++---------- src/modules/commander/gyro_calibration.cpp | 31 ++-- src/modules/commander/gyro_calibration.h | 4 +- src/modules/commander/mag_calibration.cpp | 17 +- src/modules/commander/mag_calibration.h | 4 +- src/modules/commander/rc_calibration.cpp | 13 +- src/modules/commander/rc_calibration.h | 4 +- 13 files changed, 183 insertions(+), 140 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index ddd2e23d9..c2b2e9258 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -133,7 +133,7 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp int mat_invert3(float src[3][3], float dst[3][3]); int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); -void do_accel_calibration(int mavlink_fd) { +int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); @@ -176,11 +176,11 @@ void do_accel_calibration(int mavlink_fd) { } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_positive(); + return OK; } else { /* measurements error */ mavlink_log_info(mavlink_fd, "accel calibration aborted"); - tune_negative(); + return ERROR; } /* exit accel calibration mode */ diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 3275d9461..1cf9c0977 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -45,6 +45,6 @@ #include -void do_accel_calibration(int mavlink_fd); +int do_accel_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index df08292e3..e414e5f70 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,7 +49,13 @@ #include #include -void do_airspeed_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); @@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; + return ERROR; } } @@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd) if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + return ERROR; } /* auto-save to EEPROM */ @@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } //char buf[50]; @@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_positive(); + return OK; } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + return ERROR; } - - close(diff_pres_sub); } diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 92f5651ec..71c701fc2 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -41,6 +41,6 @@ #include -void do_airspeed_calibration(int mavlink_fd); +int do_airspeed_calibration(int mavlink_fd); -#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file +#endif /* AIRSPEED_CALIBRATION_H_ */ diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index d7515b3d9..3123c4087 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -47,8 +47,14 @@ #include #include -void do_baro_calibration(int mavlink_fd) +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +int do_baro_calibration(int mavlink_fd) { // TODO implement this - return; + return ERROR; } diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index ac0f4be36..bc42bc6cf 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -41,6 +41,6 @@ #include -void do_baro_calibration(int mavlink_fd); +int do_baro_calibration(int mavlink_fd); -#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* BARO_CALIBRATION_H_ */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17db0f9c8..66b9272de 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -502,7 +502,13 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); + /* try again later */ + usleep(20000); + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + if (mavlink_fd < 0) { + warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); + } } /* Main state machine */ @@ -1628,6 +1634,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v return res; } +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) +{ + switch (result) { + case VEHICLE_CMD_RESULT_ACCEPTED: + tune_positive(); + break; + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + default: + break; + } +} + void *commander_low_prio_loop(void *arg) { /* Set thread name */ @@ -1668,125 +1701,110 @@ void *commander_low_prio_loop(void *arg) cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) continue; - /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle low-priority commands here */ switch (cmd.command) { case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(&status, &safety, &armed)) { - - if (((int)(cmd.param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd.param1)) == 3) { - /* reboot to bootloader */ - systemreset(true); - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - + if (is_safe(&status, &safety, &armed)) { + + if (((int)(cmd.param1)) == 1) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot */ + systemreset(false); + } else if (((int)(cmd.param1)) == 3) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + usleep(1000000); + /* reboot to bootloader */ + systemreset(true); } else { - result = VEHICLE_CMD_RESULT_DENIED; + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + } else { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } + break; - /* try to go to INIT/PREFLIGHT arming state */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - result = VEHICLE_CMD_RESULT_DENIED; - break; - } + int calib_ret = ERROR; - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - do_gyro_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + /* try to go to INIT/PREFLIGHT arming state */ - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - do_mag_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - result = VEHICLE_CMD_RESULT_DENIED; - - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - do_accel_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); + + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); + + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); + + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - do_airspeed_calibration(mavlink_fd); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - break; - } + break; + } case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - tune_error(); - result = VEHICLE_CMD_RESULT_FAILED; - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } - - break; } - - default: break; } - /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_positive(); - - } else { - tune_negative(); - - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - - } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); - } + default: + answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + break; } /* send any requested ACKs */ diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index f1afb0107..9cd2f3a19 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -50,8 +50,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_gyro_calibration(int mavlink_fd) +int do_gyro_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); @@ -98,7 +103,7 @@ void do_gyro_calibration(int mavlink_fd) } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; + return ERROR; } } @@ -137,18 +142,17 @@ void do_gyro_calibration(int mavlink_fd) if (save_ret != 0) { warnx("WARNING: auto-save of params to storage failed"); mavlink_log_critical(mavlink_fd, "gyro store failed"); - // XXX negative tune - return; + return ERROR; } mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); + tune_neutral(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); - return; + return ERROR; } @@ -180,8 +184,7 @@ void do_gyro_calibration(int mavlink_fd) && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); - return; + return OK; } /* wait blocking for new data */ @@ -221,7 +224,7 @@ void do_gyro_calibration(int mavlink_fd) // operating near the 1e6/1e8 max sane resolution of float. gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; - warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); +// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); // } else if (poll_ret == 0) { // /* any poll failure for 1s is a reason to abort */ @@ -232,8 +235,8 @@ void do_gyro_calibration(int mavlink_fd) float gyro_scale = baseline_integral / gyro_integral; float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; - warnx("gyro scale: yaw (z): %6.4f", gyro_scale); - mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { @@ -272,12 +275,10 @@ void do_gyro_calibration(int mavlink_fd) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_positive(); /* third beep by cal end routine */ - + return OK; } else { mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + return ERROR; } - - close(sub_sensor_combined); } diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h index cd262507d..59c32a15e 100644 --- a/src/modules/commander/gyro_calibration.h +++ b/src/modules/commander/gyro_calibration.h @@ -41,6 +41,6 @@ #include -void do_gyro_calibration(int mavlink_fd); +int do_gyro_calibration(int mavlink_fd); -#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file +#endif /* GYRO_CALIBRATION_H_ */ diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9a25103f8..9263c6a15 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -53,8 +53,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_mag_calibration(int mavlink_fd) +int do_mag_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); @@ -113,7 +118,7 @@ void do_mag_calibration(int mavlink_fd) warnx("mag cal failed: out of memory"); mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); warnx("x:%p y:%p z:%p\n", x, y, z); - return; + return ERROR; } while (hrt_absolute_time() < calibration_deadline && @@ -252,6 +257,7 @@ void do_mag_calibration(int mavlink_fd) if (save_ret != 0) { warn("WARNING: auto-save of params to storage failed"); mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + return ERROR; } warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", @@ -269,12 +275,11 @@ void do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_positive(); + return OK; /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + return ERROR; } - - close(sub_mag); -} \ No newline at end of file +} diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index fd2085f14..a101cd7b1 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -41,6 +41,6 @@ #include -void do_mag_calibration(int mavlink_fd); +int do_mag_calibration(int mavlink_fd); -#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file +#endif /* MAG_CALIBRATION_H_ */ diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 0de411713..fe87a3323 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -46,8 +46,13 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; -void do_rc_calibration(int mavlink_fd) +int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); @@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd) if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + return ERROR; } - tune_positive(); - mavlink_log_info(mavlink_fd, "trim calibration done"); -} \ No newline at end of file + return OK; +} diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h index 6505c7364..9aa6faafa 100644 --- a/src/modules/commander/rc_calibration.h +++ b/src/modules/commander/rc_calibration.h @@ -41,6 +41,6 @@ #include -void do_rc_calibration(int mavlink_fd); +int do_rc_calibration(int mavlink_fd); -#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file +#endif /* RC_CALIBRATION_H_ */ -- cgit v1.2.3 From 41fac46ff08787d9b2e4d902045d65c205389abd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 22 Aug 2013 18:05:30 +0200 Subject: mavlink VFR_HUD message fixed, minor fixes and cleanup --- src/modules/commander/commander.cpp | 4 ++-- src/modules/mavlink/orb_listener.c | 10 +++++----- .../position_estimator_inav_main.c | 2 +- src/modules/uORB/topics/vehicle_global_position.h | 18 +++++++++--------- 4 files changed, 17 insertions(+), 17 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04e6dd2cb..4580c57bc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -825,9 +825,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_landed = local_position.landed; status_changed = true; if (status.condition_landed) { - mavlink_log_info(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); } else { - mavlink_log_info(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 2a260861d..97cf571e5 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -637,12 +637,12 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); - float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; + uint16_t throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1); + float alt = global_pos.relative_alt; + float climb = -global_pos.vz; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } static void * diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3466841c4..d35755b4f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -610,7 +610,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index f036c7223..822c323cf 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,17 +62,17 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ bool valid; /**< true if position satisfies validity criteria of estimator */ - int32_t lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees LOGME */ - float alt; /**< Altitude in meters LOGME */ - float relative_alt; /**< Altitude above home position in meters, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in ENU LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in ENU LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in ENU LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + float alt; /**< Altitude in meters */ + float relative_alt; /**< Altitude above home position in meters, */ + float vx; /**< Ground X velocity, m/s in NED */ + float vy; /**< Ground Y velocity, m/s in NED */ + float vz; /**< Ground Z velocity, m/s in NED */ + float hdg; /**< Compass heading in radians -PI..+PI. */ }; -- cgit v1.2.3 From 5e9b508ea0ec799ab6f8723d114c999beffc347e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 23 Aug 2013 23:03:59 +0200 Subject: Indicate AUTO submodes in mavlink custom_mode. --- src/modules/commander/commander.cpp | 68 +++++++++++++++++--------------- src/modules/commander/px4_custom_mode.h | 29 +++++++++++--- src/modules/mavlink/mavlink.c | 24 +++++++++-- src/modules/mavlink/mavlink_receiver.cpp | 5 ++- 4 files changed, 84 insertions(+), 42 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4580c57bc..8bde6b7a9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -274,7 +274,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - uint32_t custom_mode = (uint32_t) cmd->param2; + union px4_custom_mode custom_mode; + custom_mode.data_float = cmd->param2; // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); @@ -307,19 +308,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } @@ -1544,43 +1545,46 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; - - } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } else { + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } - } - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } } + } else { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); } - break; default: diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 4d4859a5c..b60a7e0b9 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,11 +8,30 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, +enum PX4_CUSTOM_MAIN_MODE { + PX4_CUSTOM_MAIN_MODE_MANUAL = 1, + PX4_CUSTOM_MAIN_MODE_SEATBELT, + PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_AUTO, +}; + +enum PX4_CUSTOM_SUB_MODE_AUTO { + PX4_CUSTOM_SUB_MODE_AUTO_READY = 1, + PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF, + PX4_CUSTOM_SUB_MODE_AUTO_LOITER, + PX4_CUSTOM_SUB_MODE_AUTO_MISSION, + PX4_CUSTOM_SUB_MODE_AUTO_RTL, + PX4_CUSTOM_SUB_MODE_AUTO_LAND, +}; + +union px4_custom_mode { + struct { + uint16_t reserved; + uint8_t main_mode; + uint8_t sub_mode; + }; + uint32_t data; + float data_float; }; #endif /* PX4_CUSTOM_MODE_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 6d9ca1120..93ec36d05 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -205,19 +205,35 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u /* main state */ *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; + custom_mode.data = 0; if (v_status.main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; } else if (v_status.main_state == MAIN_STATE_EASY) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; } else if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; + } } + *mavlink_custom_mode = custom_mode.data; /** * Set mavlink state diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 01bbabd46..86fa73656 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -67,6 +67,7 @@ #include #include #include +#include __BEGIN_DECLS @@ -188,9 +189,11 @@ handle_message(mavlink_message_t *msg) mavlink_set_mode_t new_mode; mavlink_msg_set_mode_decode(msg, &new_mode); + union px4_custom_mode custom_mode; + custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = new_mode.custom_mode; + vcmd.param2 = custom_mode.data_float; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; -- cgit v1.2.3 From 8579d0b7c9f77c84fa9afa87f7ab2f353443a242 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 24 Aug 2013 20:31:01 +0200 Subject: Allow disarm by RC in assisted modes if landed and in AUTO_READY state. --- src/modules/commander/commander.cpp | 55 ++++++++++++++++++------------------- 1 file changed, 26 insertions(+), 29 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fb..f5a9d4088 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1017,46 +1017,42 @@ int commander_thread_main(int argc, char *argv[]) /* arm/disarm by RC */ res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && - (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); - stick_off_counter = 0; - - } else { - stick_off_counter++; - } - - stick_on_counter = 0; + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); + stick_off_counter = 0; } else { - stick_off_counter = 0; + stick_off_counter++; } + } else { + stick_off_counter = 0; } - /* check if left stick is in lower right position and we're in manual mode -> arm */ + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - status.main_state == MAIN_STATE_MANUAL) { - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - stick_on_counter = 0; - - } else { - stick_on_counter++; - } - - stick_off_counter = 0; + status.main_state == MAIN_STATE_MANUAL && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { - stick_on_counter = 0; + stick_on_counter++; } + + } else { + stick_on_counter = 0; } if (res == TRANSITION_CHANGED) { @@ -1702,7 +1698,8 @@ void *commander_low_prio_loop(void *arg) /* ignore commands the high-prio loop handles */ if (cmd.command == VEHICLE_CMD_DO_SET_MODE || - cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM) + cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || + cmd.command == VEHICLE_CMD_NAV_TAKEOFF) continue; /* only handle low-priority commands here */ -- cgit v1.2.3 From e119bbb0f1161c71b8c2dcbbbc150d40b356c4b1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Aug 2013 16:33:14 +0200 Subject: A lot more on calibration and RC checks. Needs more testing, but no known issues --- ROMFS/px4fmu_common/init.d/rcS | 6 + .../commander/accelerometer_calibration.cpp | 18 ++- src/modules/commander/commander.cpp | 6 + src/modules/commander/mag_calibration.cpp | 10 +- src/modules/sensors/sensor_params.c | 10 +- src/modules/sensors/sensors.cpp | 37 +----- src/modules/systemlib/module.mk | 3 +- src/modules/systemlib/rc_check.c | 148 +++++++++++++++++++++ src/modules/systemlib/rc_check.h | 52 ++++++++ src/systemcmds/preflight_check/preflight_check.c | 97 +------------- 10 files changed, 246 insertions(+), 141 deletions(-) create mode 100644 src/modules/systemlib/rc_check.c create mode 100644 src/modules/systemlib/rc_check.h (limited to 'src/modules/commander/commander.cpp') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 6b624b278..30edf679a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -182,6 +182,12 @@ fi # Try to get an USB console nshterm /dev/ttyACM0 & +# Start any custom extensions that might be missing +if [ -f /fs/microsd/etc/rc.local ] +then + sh /fs/microsd/etc/rc.local +fi + # If none of the autostart scripts triggered, get a minimal setup if [ $MODE == autostart ] then diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index c2b2e9258..82df7c37d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -136,6 +136,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); + mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); /* measure and calculate offsets & scales */ float accel_offs[3]; @@ -211,17 +212,28 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + unsigned done_count = 0; + while (true) { bool done = true; - char str[80]; + char str[60]; int str_ptr; str_ptr = sprintf(str, "keep vehicle still:"); + unsigned old_done_count = done_count; + done_count = 0; for (int i = 0; i < 6; i++) { if (!data_collected[i]) { str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); done = false; + } else { + done_count++; } } + + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); + if (done) break; mavlink_log_info(mavlink_fd, str); @@ -236,8 +248,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float continue; } - sprintf(str, "meas started: %s", orientation_strs[orient]); - mavlink_log_info(mavlink_fd, str); + // sprintf(str, + mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], (double)accel_ref[orient][0], diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3654839fb..e3d314881 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -617,6 +618,8 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; + bool rc_calibration_ok = (OK == rc_calibration_check()); + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); memset(&safety, 0, sizeof(safety)); @@ -727,6 +730,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); status_changed = true; + + /* Re-check RC calibration */ + rc_calibration_ok = (OK == rc_calibration_check()); } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9263c6a15..42f0190c7 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -100,6 +100,8 @@ int do_mag_calibration(int mavlink_fd) close(fd); + mavlink_log_info(mavlink_fd, "mag cal progress <20> percent"); + /* calibrate offsets */ // uint64_t calibration_start = hrt_absolute_time(); @@ -135,9 +137,8 @@ int do_mag_calibration(int mavlink_fd) axis_index++; - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter); tune_neutral(); axis_deadline += calibration_interval / 3; @@ -251,6 +252,8 @@ int do_mag_calibration(int mavlink_fd) warnx("Setting Z mag scale failed!\n"); } + mavlink_log_info(mavlink_fd, "mag cal progress <90> percent"); + /* auto-save to EEPROM */ int save_ret = param_save_default(); @@ -274,6 +277,7 @@ int do_mag_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "mag calibration done"); + mavlink_log_info(mavlink_fd, "mag cal progress <100> percent"); return OK; /* third beep by cal end routine */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index b45317dbe..fd2a6318e 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -157,7 +157,6 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000); PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); -PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ @@ -184,10 +183,7 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera trigger */ -PARAM_DEFINE_INT32(RC_MAP_AUX4, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX5, 0); /**< default function: payload drop */ -PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); -PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 1.0f); +PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); +PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ded39c465..e857b1c2f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -305,8 +305,6 @@ private: int board_rotation; int external_mag_rotation; - int rc_type; - int rc_map_roll; int rc_map_pitch; int rc_map_yaw; @@ -342,9 +340,6 @@ private: param_t max[_rc_max_chan_count]; param_t rev[_rc_max_chan_count]; param_t dz[_rc_max_chan_count]; - param_t rc_type; - - param_t rc_demix; param_t gyro_offset[3]; param_t gyro_scale[3]; @@ -566,8 +561,6 @@ Sensors::Sensors() : } - _parameter_handles.rc_type = param_find("RC_TYPE"); - /* mandatory input switched, mapped to channels 1-4 per default */ _parameter_handles.rc_map_roll = param_find("RC_MAP_ROLL"); _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); @@ -692,11 +685,6 @@ Sensors::parameters_update() if (!rc_valid) warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); - /* remote control type */ - if (param_get(_parameter_handles.rc_type, &(_parameters.rc_type)) != OK) { - warnx("Failed getting remote control type"); - } - /* channel mapping */ if (param_get(_parameter_handles.rc_map_roll, &(_parameters.rc_map_roll)) != OK) { warnx("Failed getting roll chan index"); @@ -738,26 +726,11 @@ Sensors::parameters_update() // warnx("Failed getting offboard control mode sw chan index"); // } - if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { - warnx("Failed getting mode aux 1 index"); - } - - if (param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)) != OK) { - warnx("Failed getting mode aux 2 index"); - } - - if (param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)) != OK) { - warnx("Failed getting mode aux 3 index"); - } - - if (param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)) != OK) { - warnx("Failed getting mode aux 4 index"); - } - - if (param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)) != OK) { - warnx("Failed getting mode aux 5 index"); - } - + param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); + param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); + param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); + param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); + param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_scale_roll, &(_parameters.rc_scale_roll)); param_get(_parameter_handles.rc_scale_pitch, &(_parameters.rc_scale_pitch)); param_get(_parameter_handles.rc_scale_yaw, &(_parameters.rc_scale_yaw)); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 94c744c03..843cda722 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ systemlib.c \ airspeed.c \ system_params.c \ - mavlink_log.c + mavlink_log.c \ + rc_check.c diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c new file mode 100644 index 000000000..9d47d8000 --- /dev/null +++ b/src/modules/systemlib/rc_check.c @@ -0,0 +1,148 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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. + * + ****************************************************************************/ + +/** + * @file rc_check.c + * + * RC calibration check + */ + +#include + +#include +#include + +#include +#include +#include +#include + +int rc_calibration_check(void) { + + char nbuf[20]; + param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, + _parameter_handles_rev, _parameter_handles_dz; + + int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + float param_min, param_max, param_trim, param_rev, param_dz; + + /* first check channel mappings */ + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + + + for (int i = 0; i < RC_CHANNELS_MAX; i++) { + /* should the channel be enabled? */ + uint8_t count = 0; + + /* min values */ + sprintf(nbuf, "RC%d_MIN", i + 1); + _parameter_handles_min = param_find(nbuf); + param_get(_parameter_handles_min, ¶m_min); + + /* trim values */ + sprintf(nbuf, "RC%d_TRIM", i + 1); + _parameter_handles_trim = param_find(nbuf); + param_get(_parameter_handles_trim, ¶m_trim); + + /* max values */ + sprintf(nbuf, "RC%d_MAX", i + 1); + _parameter_handles_max = param_find(nbuf); + param_get(_parameter_handles_max, ¶m_max); + + /* channel reverse */ + sprintf(nbuf, "RC%d_REV", i + 1); + _parameter_handles_rev = param_find(nbuf); + param_get(_parameter_handles_rev, ¶m_rev); + + /* channel deadzone */ + sprintf(nbuf, "RC%d_DZ", i + 1); + _parameter_handles_dz = param_find(nbuf); + param_get(_parameter_handles_dz, ¶m_dz); + + /* assert min..center..max ordering */ + if (param_min < 500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_max > 2500) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim < param_min) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + if (param_trim > param_max) { + count++; + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + } + + /* assert deadzone is sane */ + if (param_dz > 500) { + mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); + /* give system time to flush error message in case there are more */ + usleep(100000); + count++; + } + + /* check which map param applies */ + // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { + // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); + // /* give system time to flush error message in case there are more */ + // usleep(100000); + // count++; + // } + + /* sanity checks pass, enable channel */ + if (count) { + mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); + usleep(100000); + } + } +} diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h new file mode 100644 index 000000000..e2238d151 --- /dev/null +++ b/src/modules/systemlib/rc_check.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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. + * + ****************************************************************************/ + +/** + * @file rc_check.h + * + * RC calibration check + */ + +#pragma once + + __BEGIN_DECLS + +/** + * Check the RC calibration + * + * @return 0 / OK if RC calibration is ok, index + 1 of the first + * channel that failed else (so 1 == first channel failed) + */ +__EXPORT int rc_calibration_check(void); + +__END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e7d9ce85f..4c19dcffb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -57,6 +57,7 @@ #include #include +#include __EXPORT int preflight_check_main(int argc, char *argv[]); static int led_toggle(int leds, int led); @@ -139,101 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, - _parameter_handles_rev, _parameter_handles_dz; - - float param_min, param_max, param_trim, param_rev, param_dz; - - bool rc_ok = true; - char nbuf[20]; - - /* first check channel mappings */ - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - for (int i = 0; i < 12; i++) { - /* should the channel be enabled? */ - uint8_t count = 0; - - /* min values */ - sprintf(nbuf, "RC%d_MIN", i + 1); - _parameter_handles_min = param_find(nbuf); - param_get(_parameter_handles_min, ¶m_min); - - /* trim values */ - sprintf(nbuf, "RC%d_TRIM", i + 1); - _parameter_handles_trim = param_find(nbuf); - param_get(_parameter_handles_trim, ¶m_trim); - - /* max values */ - sprintf(nbuf, "RC%d_MAX", i + 1); - _parameter_handles_max = param_find(nbuf); - param_get(_parameter_handles_max, ¶m_max); - - /* channel reverse */ - sprintf(nbuf, "RC%d_REV", i + 1); - _parameter_handles_rev = param_find(nbuf); - param_get(_parameter_handles_rev, ¶m_rev); - - /* channel deadzone */ - sprintf(nbuf, "RC%d_DZ", i + 1); - _parameter_handles_dz = param_find(nbuf); - param_get(_parameter_handles_dz, ¶m_dz); - - /* assert min..center..max ordering */ - if (param_min < 500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MIN < 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_max > 2500) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAX > 2500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim < param_min) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - if (param_trim > param_max) { - count++; - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - } - - /* assert deadzone is sane */ - if (param_dz > 500) { - mavlink_log_critical(mavlink_fd, "ERR: RC_%d_DZ > 500", i+1); - /* give system time to flush error message in case there are more */ - usleep(100000); - count++; - } - - /* check which map param applies */ - // if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) { - // mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1); - // /* give system time to flush error message in case there are more */ - // usleep(100000); - // count++; - // } - - /* sanity checks pass, enable channel */ - if (count) { - mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1)); - usleep(100000); - rc_ok = false; - } - } + bool rc_ok = (OK == rc_calibration_check()); /* require RC ok to keep system_ok */ system_ok &= rc_ok; -- cgit v1.2.3 From 725bb7697c9fc85ac95fdadc9d2fd2ef5b9848f1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 25 Aug 2013 20:17:42 +0200 Subject: Minor fix in "set mode" command handling. --- src/modules/commander/commander.cpp | 13 ++++++------- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 2 files changed, 7 insertions(+), 8 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f7ac24341..74cd5e36a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -323,11 +323,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; - union px4_custom_mode custom_mode; - custom_mode.data_float = cmd->param2; + uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -357,19 +356,19 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ - if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { /* SEATBELT */ main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (custom_mode.main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index eb28af1a1..af43542da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -201,7 +201,7 @@ handle_message(mavlink_message_t *msg) custom_mode.data = new_mode.custom_mode; /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = new_mode.base_mode; - vcmd.param2 = custom_mode.data_float; + vcmd.param2 = custom_mode.main_mode; vcmd.param3 = 0; vcmd.param4 = 0; vcmd.param5 = 0; -- cgit v1.2.3 From c5731bbc3f29361f3d50ecc54d44a521d2441a48 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 09:12:17 +0200 Subject: TAKEOFF implemented for multirotors, added altitude check to waypoint navigation. --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 2 +- src/modules/commander/commander.cpp | 14 ++- src/modules/commander/state_machine_helper.cpp | 1 + src/modules/mavlink/orb_listener.c | 2 +- src/modules/mavlink/waypoints.c | 40 ++++---- .../multirotor_pos_control.c | 101 +++++++++++++-------- .../position_estimator_inav_main.c | 3 +- src/modules/uORB/topics/vehicle_control_mode.h | 2 + src/modules/uORB/topics/vehicle_global_position.h | 3 +- src/modules/uORB/topics/vehicle_local_position.h | 2 + 10 files changed, 101 insertions(+), 69 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 191d20f30..33879892e 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -325,7 +325,7 @@ void KalmanNav::updatePublications() _pos.vx = vN; _pos.vy = vE; _pos.vz = vD; - _pos.hdg = psi; + _pos.yaw = psi; // attitude publication _att.timestamp = _pubTimeStamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 74cd5e36a..39d74e37a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -210,7 +210,7 @@ void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -1214,7 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) } /* evaluate the navigation state machine */ - transition_result_t res = check_navigation_state_machine(&status, &control_mode); + transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1572,7 +1572,7 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) { transition_result_t res = TRANSITION_DENIED; @@ -1592,8 +1592,12 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + /* check if takeoff completed */ + if (local_pos->z < -5.0f && !status.condition_landed) { + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } else { + res = TRANSITION_NOT_CHANGED; + } } else { if (current_status->condition_landed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 80ee3db23..fe7e8cc53 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -413,6 +413,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + control_mode->auto_state = current_status->navigation_state; navigation_state_changed = true; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index dcdc03281..53d86ec00 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -341,7 +341,7 @@ l_global_position(const struct listener *l) int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); + uint16_t hdg = (global_pos.yaw / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index eea928a17..16a7c2d35 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -294,16 +294,13 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) */ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) { - //TODO: implement for z once altidude contoller is implemented - static uint16_t counter; -// if(counter % 10 == 0) printf(" x = %.10f, y = %.10f\n", x, y); if (seq < wpm->size) { - mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); + mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); - double current_x_rad = cur->x / 180.0 * M_PI; - double current_y_rad = cur->y / 180.0 * M_PI; + double current_x_rad = wp->x / 180.0 * M_PI; + double current_y_rad = wp->y / 180.0 * M_PI; double x_rad = lat / 180.0 * M_PI; double y_rad = lon / 180.0 * M_PI; @@ -315,7 +312,10 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float const double radius_earth = 6371000.0; - return radius_earth * c; + float dxy = radius_earth * c; + float dz = alt - wp->z; + + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; @@ -383,21 +383,19 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ // XXX TODO } - if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw - + if (dist >= 0.f && dist <= orbit) { wpm->pos_reached = true; - } - -// else -// { -// if(counter % 100 == 0) -// printf("Setpoint not reached yet: %0.4f, orbit: %.4f, coordinate frame: %d\n",dist, orbit, coordinate_frame); -// } + // check if required yaw reached + float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); + float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); + if (fabsf(yaw_err) < 0.05f) { + wpm->yaw_reached = true; + } } //check if the current waypoint was reached - if (wpm->pos_reached /*wpm->yaw_reached &&*/ && !wpm->idle) { + if (wpm->pos_reached && /*wpm->yaw_reached &&*/ !wpm->idle) { if (wpm->current_active_wp_id < wpm->size) { mavlink_mission_item_t *cur_wp = &(wpm->waypoints[wpm->current_active_wp_id]); @@ -412,11 +410,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ bool time_elapsed = false; - if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) { - if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { - time_elapsed = true; - } - } else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { + if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) { time_elapsed = true; } else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) { time_elapsed = true; @@ -493,7 +487,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio // mavlink_wpm_send_setpoint(wpm->current_active_wp_id); // } - check_waypoints_reached(now, global_position , local_position); + check_waypoints_reached(now, global_position, local_position); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index a6ebeeacb..e65792c76 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include @@ -221,11 +222,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_int_xy = true; bool was_armed = false; bool reset_integral = true; + bool reset_auto_pos = true; hrt_abstime t_prev = 0; - /* integrate in NED frame to estimate wind but not attitude offset */ const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + const float takeoff_alt_default = 10.0f; float ref_alt = 0.0f; hrt_abstime ref_alt_t = 0; uint64_t local_ref_timestamp = 0; @@ -414,50 +416,76 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, use global setpoint */ - /* init local projection using local position ref */ - if (local_pos.ref_timestamp != local_ref_timestamp) { - global_pos_sp_reproject = true; - local_ref_timestamp = local_pos.ref_timestamp; - double lat_home = local_pos.ref_lat * 1e-7; - double lon_home = local_pos.ref_lon * 1e-7; - map_projection_init(lat_home, lon_home); - mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); - } - - if (global_pos_sp_reproject) { - /* update global setpoint projection */ - global_pos_sp_reproject = false; + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = -takeoff_alt_default; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { + if (reset_auto_pos) { + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + att_sp.yaw_body = att.yaw; + reset_auto_pos = false; + } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { + // TODO + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { + /* init local projection using local position ref */ + if (local_pos.ref_timestamp != local_ref_timestamp) { + global_pos_sp_reproject = true; + local_ref_timestamp = local_pos.ref_timestamp; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; + map_projection_init(lat_home, lon_home); + mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home); + } - if (global_pos_sp_valid) { - /* global position setpoint valid, use it */ - double sp_lat = global_pos_sp.lat * 1e-7; - double sp_lon = global_pos_sp.lon * 1e-7; - /* project global setpoint to local setpoint */ - map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp_reproject) { + /* update global setpoint projection */ + global_pos_sp_reproject = false; + if (global_pos_sp_valid) { + /* global position setpoint valid, use it */ + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + + } else { + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; + } - if (global_pos_sp.altitude_is_relative) { - local_pos_sp.z = -global_pos_sp.altitude; + mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); } else { - local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + if (!local_pos_sp_valid) { + /* local position setpoint is invalid, + * use current position as setpoint for loiter */ + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.z = local_pos.z; + } - } else { - if (!local_pos_sp_valid) { - /* local position setpoint is invalid, - * use current position as setpoint for loiter */ - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + att_sp.yaw_body = global_pos_sp.yaw; + reset_auto_pos = true; + } - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { + global_pos_sp_reproject = true; } /* reset setpoints after non-manual modes */ @@ -575,6 +603,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_int_z = true; reset_int_xy = true; global_pos_sp_reproject = true; + reset_auto_pos = true; } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 1e20f9de9..af6a704a2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -591,6 +591,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.z = z_est[0]; local_pos.vz = z_est[1]; local_pos.landed = landed; + local_pos.yaw = att.yaw; orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); @@ -609,7 +610,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_xy_valid) { global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; - global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); // TODO is it correct? } if (local_pos.z_valid) { @@ -623,6 +623,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.v_z_valid) { global_pos.vz = local_pos.vz; } + global_pos.yaw = local_pos.yaw; global_pos.timestamp = t; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 4f4db5dbc..e15ddde26 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -82,6 +82,8 @@ struct vehicle_control_mode_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ + + uint8_t auto_state; // TEMP navigation state for AUTO modes }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 822c323cf..0fc0ed5c9 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -72,8 +72,7 @@ struct vehicle_global_position_s float vx; /**< Ground X velocity, m/s in NED */ float vy; /**< Ground Y velocity, m/s in NED */ float vz; /**< Ground Z velocity, m/s in NED */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - + float yaw; /**< Compass heading in radians -PI..+PI. */ }; /** diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 26e8f335b..31a0e632b 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -67,6 +67,8 @@ struct vehicle_local_position_s float vx; /**< Ground X Speed (Latitude), m/s in NED */ float vy; /**< Ground Y Speed (Longitude), m/s in NED */ float vz; /**< Ground Z Speed (Altitude), m/s in NED */ + /* Heading */ + float yaw; /* Reference position in GPS / WGS84 frame */ bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ -- cgit v1.2.3 From baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:55 +0200 Subject: commander: do AUTO_MISSION after takeoff --- src/modules/commander/commander.cpp | 54 ++++++++++++++++++------------------- 1 file changed, 26 insertions(+), 28 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39d74e37a..d90008633 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* check if takeoff completed */ - if (local_pos->z < -5.0f && !status.condition_landed) { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } else { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -5.0f || status.condition_landed) { res = TRANSITION_NOT_CHANGED; + break; } + } + /* check again, state can be changed */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } - - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } + + } else { + /* switch to mission in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } } else { -- cgit v1.2.3 From 66c61fbe96e11ee7099431a8370d84f862543810 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 27 Aug 2013 23:08:00 +0200 Subject: Full failsafe rewrite. --- src/modules/commander/commander.cpp | 587 ++++++++++----------- src/modules/commander/commander_params.c | 2 +- src/modules/commander/state_machine_helper.cpp | 30 +- src/modules/commander/state_machine_helper.h | 6 +- src/modules/mavlink/mavlink.c | 6 +- .../multirotor_att_control_main.c | 98 ++-- .../multirotor_pos_control.c | 59 ++- .../position_estimator_inav_main.c | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 4 +- src/modules/uORB/topics/vehicle_status.h | 13 +- 10 files changed, 394 insertions(+), 413 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d90008633..a548f943e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -147,8 +147,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* timout until lowlevel failsafe */ -static unsigned int failsafe_lowlevel_timeout_ms; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; @@ -199,6 +197,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode int commander_thread_main(int argc, char *argv[]); void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); + void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -206,17 +205,20 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); + void print_reject_arm(const char *msg); void print_status(); -transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ void *commander_low_prio_loop(void *arg); +void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result); + int commander_main(int argc, char *argv[]) { @@ -271,7 +273,8 @@ void usage(const char *reason) exit(1); } -void print_status() { +void print_status() +{ warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ @@ -279,33 +282,40 @@ void print_status() { struct vehicle_status_s state; orb_copy(ORB_ID(vehicle_status), state_sub, &state); - const char* armed_str; + const char *armed_str; switch (state.arming_state) { - case ARMING_STATE_INIT: - armed_str = "INIT"; - break; - case ARMING_STATE_STANDBY: - armed_str = "STANDBY"; - break; - case ARMING_STATE_ARMED: - armed_str = "ARMED"; - break; - case ARMING_STATE_ARMED_ERROR: - armed_str = "ARMED_ERROR"; - break; - case ARMING_STATE_STANDBY_ERROR: - armed_str = "STANDBY_ERROR"; - break; - case ARMING_STATE_REBOOT: - armed_str = "REBOOT"; - break; - case ARMING_STATE_IN_AIR_RESTORE: - armed_str = "IN_AIR_RESTORE"; - break; - default: - armed_str = "ERR: UNKNOWN STATE"; - break; + case ARMING_STATE_INIT: + armed_str = "INIT"; + break; + + case ARMING_STATE_STANDBY: + armed_str = "STANDBY"; + break; + + case ARMING_STATE_ARMED: + armed_str = "ARMED"; + break; + + case ARMING_STATE_ARMED_ERROR: + armed_str = "ARMED_ERROR"; + break; + + case ARMING_STATE_STANDBY_ERROR: + armed_str = "STANDBY_ERROR"; + break; + + case ARMING_STATE_REBOOT: + armed_str = "REBOOT"; + break; + + case ARMING_STATE_IN_AIR_RESTORE: + armed_str = "IN_AIR_RESTORE"; + break; + + default: + armed_str = "ERR: UNKNOWN STATE"; + break; } @@ -326,7 +336,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t custom_main_mode = (uint8_t) cmd->param2; // TODO remove debug code - mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -415,6 +425,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } else { /* reject TAKEOFF not armed */ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; @@ -478,9 +489,6 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; /* set parameters */ - failsafe_lowlevel_timeout_ms = 0; - param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); - param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); @@ -599,12 +607,6 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_off_counter = 0; unsigned stick_on_counter = 0; - /* To remember when last notification was sent */ - uint64_t last_print_control_time = 0; - - enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; - bool armed_previous = false; - bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; @@ -665,7 +667,6 @@ int commander_thread_main(int argc, char *argv[]) int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; memset(&diff_pres, 0, sizeof(diff_pres)); - uint64_t last_diff_pres_time = 0; /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -800,12 +801,15 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; + if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + } else { mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); } @@ -908,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) // XXX should we still allow to arm with critical battery? //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } + status_changed = true; } @@ -943,11 +948,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - - /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph_m; - float vdop_m = gps_position.epv_m; - /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -963,7 +963,7 @@ int commander_thread_main(int argc, char *argv[]) */ if (!home_position_set && gps_position.fix_type >= 3 && - (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate @@ -1013,9 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) } } - status.rc_signal_cutting_off = false; status.rc_signal_lost = false; - status.rc_signal_lost_interval = 0; transition_result_t res; // store all transitions results here @@ -1027,10 +1025,10 @@ int commander_thread_main(int argc, char *argv[]) if (status.is_rotary_wing && (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY || - (status.condition_landed && ( - status.navigation_state == NAVIGATION_STATE_ALTHOLD || - status.navigation_state == NAVIGATION_STATE_VECTOR - ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.condition_landed && ( + status.navigation_state == NAVIGATION_STATE_ALTHOLD || + status.navigation_state == NAVIGATION_STATE_VECTOR + ))) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); @@ -1040,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) } else { stick_off_counter++; } + } else { stick_off_counter = 0; } @@ -1087,7 +1086,7 @@ int commander_thread_main(int argc, char *argv[]) res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); tune_positive(); } else if (res == TRANSITION_DENIED) { @@ -1097,122 +1096,14 @@ int commander_thread_main(int argc, char *argv[]) } } else { - - /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { - // TODO remove debug code - if (!status.rc_signal_cutting_off) { - warnx("Reason: not rc_signal_cutting_off\n"); - - } else { - warnx("last print time: %llu\n", last_print_control_time); - } - - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; - - /* if the RC signal is gone for a full second, consider it lost */ - if (status.rc_signal_lost_interval > 1000000) { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; - status.failsave_lowlevel = true; status_changed = true; } } } - /* END mode switch */ - /* END RC state check */ - - // TODO check this - /* state machine update for offboard control */ - if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? - - // /* decide about attitude control flag, enable in att/pos/vel */ - // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - - // /* decide about rate control flag, enable it always XXX (for now) */ - // bool rates_ctrl_enabled = true; - - // /* set up control mode */ - // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - // state_changed = true; - // } - - // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - // current_status.flag_control_rates_enabled = rates_ctrl_enabled; - // state_changed = true; - // } - - // /* handle the case where offboard control signal was regained */ - // if (!current_status.offboard_control_signal_found_once) { - // current_status.offboard_control_signal_found_once = true; - // /* enable offboard control, disable manual input */ - // current_status.flag_control_manual_enabled = false; - // current_status.flag_control_offboard_enabled = true; - // state_changed = true; - // tune_positive(); - - // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - - // } else { - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - // state_changed = true; - // tune_positive(); - // } - // } - - status.offboard_control_signal_weak = false; - status.offboard_control_signal_lost = false; - status.offboard_control_signal_lost_interval = 0; - - // XXX check if this is correct - /* arm / disarm on request */ - if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - } - - } else { - - /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { - status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = hrt_absolute_time(); - } - - /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; - - /* if the signal is gone for 0.1 seconds, consider it lost */ - if (status.offboard_control_signal_lost_interval > 100000) { - status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_positive(); - - /* kill motors after timeout */ - if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { - status.failsave_lowlevel = true; - status_changed = true; - } - } - } - } - /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); @@ -1230,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { status_changed = false; } @@ -1288,10 +1180,6 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } - /* store old modes to detect and act on state transitions */ - battery_warning_previous = status.battery_warning; - armed_previous = armed.armed; - fflush(stdout); counter++; @@ -1343,6 +1231,7 @@ void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ if (armed->armed) { /* armed, solid */ @@ -1352,11 +1241,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE); + } else { /* not ready to arm, blink at 10Hz */ if (leds_counter % 2 == 0) led_toggle(LED_BLUE); } + #endif if (changed) { @@ -1369,50 +1260,60 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* armed, solid */ if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN; + pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } + pattern.duration[0] = 1000; } else if (armed->ready_to_arm) { - for (i=0; i<3; i++) { + for (i = 0; i < 3; i++) { if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + } else { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; } - pattern.duration[i*2] = 200; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; + pattern.duration[i * 2] = 200; + + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; } + if (status->condition_global_position_valid) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 1000; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 800; + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 1000; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 800; + } else { - for (i=3; i<6; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i*2] = 100; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 100; + for (i = 3; i < 6; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; + pattern.duration[i * 2] = 100; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 100; } - pattern.color[6*2] = RGBLED_COLOR_OFF; - pattern.duration[6*2] = 700; + + pattern.color[6 * 2] = RGBLED_COLOR_OFF; + pattern.duration[6 * 2] = 700; } } else { - for (i=0; i<3; i++) { - pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i*2] = 200; - pattern.color[i*2+1] = RGBLED_COLOR_OFF; - pattern.duration[i*2+1] = 200; + for (i = 0; i < 3; i++) { + pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; + pattern.duration[i * 2] = 200; + pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; + pattern.duration[i * 2 + 1] = 200; } + /* not ready to arm, blink at 10Hz */ } @@ -1423,6 +1324,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (status->load > 0.95f) { if (leds_counter % 2 == 0) led_toggle(LED_AMBER); + } else { led_off(LED_AMBER); } @@ -1572,70 +1474,124 @@ print_reject_arm(const char *msg) } transition_result_t -check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) +check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos) { transition_result_t res = TRANSITION_DENIED; - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; - - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; - - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; - - case MAIN_STATE_AUTO: - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (status->main_state == MAIN_STATE_AUTO) { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status.condition_landed) { + if (local_pos->z > -5.0f || status->condition_landed) { res = TRANSITION_NOT_CHANGED; - break; } } - /* check again, state can be changed */ - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + if (res != TRANSITION_NOT_CHANGED) { + /* check again, state can be changed */ + if (status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + + } else { + /* not landed */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); + + } else { + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } + } } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* switch to MISSION in air when no RC control */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } + } + } + + } else { + /* disarmed, always switch to AUTO_READY */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); + } + + } else { + /* manual control modes */ + if (status->rc_signal_lost && (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR)) { + /* switch to failsafe mode */ + bool manual_control_old = control_mode->flag_control_manual_enabled; + + if (!status->condition_landed) { + /* in air: try to hold position */ + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + + } else { + /* landed: don't try to hold position but land (if taking off) */ + res = TRANSITION_DENIED; + } + + if (res == TRANSITION_DENIED) { + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + } + + control_mode->flag_control_manual_enabled = false; + + if (res == TRANSITION_NOT_CHANGED && manual_control_old) { + /* mark navigation state as changed to force immediate flag publishing */ + set_navigation_state_changed(); + res = TRANSITION_CHANGED; + } + + if (res == TRANSITION_CHANGED) { + if (control_mode->flag_control_position_enabled) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); } else { - /* switch to mission in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + if (status->condition_landed) { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + + } else { + mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + } } } + } else { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - } - break; + switch (status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(status, status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; - default: - break; + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(status, NAVIGATION_STATE_VECTOR, control_mode); + break; + + default: + break; + } + } } return res; @@ -1644,27 +1600,32 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { - case VEHICLE_CMD_RESULT_ACCEPTED: + case VEHICLE_CMD_RESULT_ACCEPTED: tune_positive(); - break; - case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); - tune_negative(); - break; - case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); - tune_negative(); - break; - default: - break; + break; + + case VEHICLE_CMD_RESULT_DENIED: + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_FAILED: + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + tune_negative(); + break; + + case VEHICLE_CMD_RESULT_UNSUPPORTED: + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + tune_negative(); + break; + + default: + break; } } @@ -1720,11 +1681,13 @@ void *commander_low_prio_loop(void *arg) usleep(1000000); /* reboot */ systemreset(false); + } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); usleep(1000000); /* reboot to bootloader */ systemreset(true); + } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } @@ -1732,83 +1695,85 @@ void *commander_low_prio_loop(void *arg) } else { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); } - break; + + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - int calib_ret = ERROR; + int calib_ret = ERROR; - /* try to go to INIT/PREFLIGHT arming state */ + /* try to go to INIT/PREFLIGHT arming state */ - // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - break; - } + // XXX disable interrupts in arming_state_transition + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + break; + } - if ((int)(cmd.param1) == 1) { - /* gyro calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_gyro_calibration(mavlink_fd); + if ((int)(cmd.param1) == 1) { + /* gyro calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_gyro_calibration(mavlink_fd); - } else if ((int)(cmd.param2) == 1) { - /* magnetometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_mag_calibration(mavlink_fd); + } else if ((int)(cmd.param2) == 1) { + /* magnetometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_mag_calibration(mavlink_fd); - } else if ((int)(cmd.param3) == 1) { - /* zero-altitude pressure calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } else if ((int)(cmd.param3) == 1) { + /* zero-altitude pressure calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if ((int)(cmd.param4) == 1) { - /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + } else if ((int)(cmd.param4) == 1) { + /* RC calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); - } else if ((int)(cmd.param5) == 1) { - /* accelerometer calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_accel_calibration(mavlink_fd); + } else if ((int)(cmd.param5) == 1) { + /* accelerometer calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_accel_calibration(mavlink_fd); - } else if ((int)(cmd.param6) == 1) { - /* airspeed calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_airspeed_calibration(mavlink_fd); - } + } else if ((int)(cmd.param6) == 1) { + /* airspeed calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_airspeed_calibration(mavlink_fd); + } - if (calib_ret == OK) - tune_positive(); - else - tune_negative(); + if (calib_ret == OK) + tune_positive(); + else + tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - break; - } + break; + } case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + if (((int)(cmd.param1)) == 0) { + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); - } + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } - } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); - answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + } else if (((int)(cmd.param1)) == 1) { + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); - answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } else { + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); + } } + + break; } - break; - } default: answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); @@ -1817,7 +1782,7 @@ void *commander_low_prio_loop(void *arg) /* send any requested ACKs */ if (cmd.confirmation > 0 && cmd.command != VEHICLE_CMD_DO_SET_MODE - && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { + && cmd.command != VEHICLE_CMD_COMPONENT_ARM_DISARM) { /* send acknowledge command */ // XXX TODO } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 6832fc5ce..0a1703b2e 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,7 @@ #include #include -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index fe7e8cc53..674f3feda 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * return ret; } -bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safety, const struct actuator_armed_s *armed) { // System is safe if: // 1) Not armed @@ -276,12 +276,12 @@ check_main_state_changed() } transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) +navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_status->navigation_state) { + if (new_navigation_state == status->navigation_state) { ret = TRANSITION_NOT_CHANGED; } else { @@ -296,6 +296,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_STABILIZE: @@ -307,6 +308,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_ALTHOLD: @@ -318,6 +320,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_VECTOR: @@ -329,6 +332,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_READY: @@ -340,12 +344,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -354,6 +359,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -367,6 +373,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: @@ -378,6 +385,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_RTL: @@ -389,12 +397,13 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LAND: /* deny transitions from landed state */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; @@ -403,6 +412,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; } break; @@ -412,8 +422,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } if (ret == TRANSITION_CHANGED) { - current_status->navigation_state = new_navigation_state; - control_mode->auto_state = current_status->navigation_state; + status->navigation_state = new_navigation_state; + control_mode->auto_state = status->navigation_state; navigation_state_changed = true; } } @@ -433,6 +443,12 @@ check_navigation_state_changed() } } +void +set_navigation_state_changed() +{ + navigation_state_changed = true; +} + /** * Transition from one hil state to another */ diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index a38c2497e..1641b6f60 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); @@ -68,10 +68,12 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); bool check_navigation_state_changed(); +void set_navigation_state_changed(); + int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 0a506b1a9..d7b0fa9c7 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -240,11 +240,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u **/ /* set calibration state */ - if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_INIT + if (v_status.arming_state == ARMING_STATE_INIT || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index b3669d9ff..04582f2a4 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -2,6 +2,7 @@ * * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,6 +39,7 @@ * Implementation of multirotor attitude control main loop. * * @author Lorenz Meier + * @author Anton Babushkin */ #include @@ -63,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -74,8 +77,6 @@ #include "multirotor_attitude_control.h" #include "multirotor_rate_control.h" -PARAM_DEFINE_FLOAT(MC_RCLOSS_THR, 0.0f); // This defines the throttle when the RC signal is lost. - __EXPORT int multirotor_att_control_main(int argc, char *argv[]); static bool thread_should_exit; @@ -102,7 +103,8 @@ mc_thread_main(int argc, char *argv[]) memset(&offboard_sp, 0, sizeof(offboard_sp)); struct vehicle_rates_setpoint_s rates_sp; memset(&rates_sp, 0, sizeof(rates_sp)); - + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); @@ -114,6 +116,8 @@ mc_thread_main(int argc, char *argv[]) int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + int status_sub = orb_subscribe(ORB_ID(vehicle_status)); /* * Do not rate-limit the loop to prevent aliasing @@ -136,7 +140,6 @@ mc_thread_main(int argc, char *argv[]) orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); orb_advert_t rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - int rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control_runtime"); @@ -149,12 +152,6 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool control_yaw_position = true; bool reset_yaw_sp = true; - bool failsafe_first_time = true; - - /* prepare the handle for the failsafe throttle */ - param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); - float failsafe_throttle = 0.0f; - param_get(failsafe_throttle_handle, &failsafe_throttle); while (!thread_should_exit) { @@ -176,7 +173,6 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_sub, &update); /* update parameters */ - param_get(failsafe_throttle_handle, &failsafe_throttle); } /* only run controller if attitude changed */ @@ -205,6 +201,13 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), setpoint_sub, &offboard_sp); } + /* get a local copy of status */ + orb_check(status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), status_sub, &status); + } + /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); @@ -244,47 +247,18 @@ mc_thread_main(int argc, char *argv[]) /* manual input */ if (control_mode.flag_control_attitude_enabled) { /* control attitude, update attitude setpoint depending on mode */ - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (control_mode.failsave_highlevel) { - failsafe_first_time = false; - - if (!control_mode.flag_control_velocity_enabled) { - /* don't reset attitude setpoint in position control mode, it's handled by position controller. */ - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - } - - if (!control_mode.flag_control_climb_rate_enabled) { - /* don't touch throttle in modes with altitude hold, it's handled by position controller. - * - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > min_takeoff_throttle) { // TODO use landed status instead of throttle - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - } + if (att_sp.thrust < 0.1f) { + /* no thrust, don't try to control yaw */ + rates_sp.yaw = 0.0f; + control_yaw_position = false; - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (failsafe_first_time) { + if (status.condition_landed) { + /* reset yaw setpoint if on ground */ reset_yaw_sp = true; } } else { - failsafe_first_time = true; - - /* only move yaw setpoint if manual input is != 0 and not landed */ + /* only move yaw setpoint if manual input is != 0 */ if (manual.yaw < -yaw_deadzone || yaw_deadzone < manual.yaw) { /* control yaw rate */ control_yaw_position = false; @@ -294,18 +268,17 @@ mc_thread_main(int argc, char *argv[]) } else { control_yaw_position = true; } + } - if (!control_mode.flag_control_velocity_enabled) { - /* update attitude setpoint if not in position control mode */ - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; + if (!control_mode.flag_control_velocity_enabled) { + /* update attitude setpoint if not in position control mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_climb_rate_enabled) { - /* pass throttle directly if not in altitude control mode */ - att_sp.thrust = manual.throttle; - } + if (!control_mode.flag_control_climb_rate_enabled) { + /* pass throttle directly if not in altitude control mode */ + att_sp.thrust = manual.throttle; } - } /* reset yaw setpint to current position if needed */ @@ -324,7 +297,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.timestamp = hrt_absolute_time(); - /* publish the controller output */ + /* publish the attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } else { @@ -342,6 +315,17 @@ mc_thread_main(int argc, char *argv[]) } } else { + if (!control_mode.flag_control_auto_enabled) { + /* no control, try to stay on place */ + if (!control_mode.flag_control_velocity_enabled) { + /* no velocity control, reset attitude setpoint */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + } + /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 385892f04..8227f76c5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,10 +415,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; - } else { - /* non-manual mode, use global setpoint */ + } else if (control_mode.flag_control_auto_enabled) { + /* AUTO mode, use global setpoint */ if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; @@ -428,21 +429,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); - } - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { - if (reset_auto_pos) { - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = local_pos.z; - local_pos_sp.yaw = att.yaw; - local_pos_sp_valid = true; - att_sp.yaw_body = att.yaw; - reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z); } + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { // TODO reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_MISSION) { /* init local projection using local position ref */ if (local_pos.ref_timestamp != local_ref_timestamp) { @@ -457,6 +450,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (global_pos_sp_reproject) { /* update global setpoint projection */ global_pos_sp_reproject = false; + if (global_pos_sp_valid) { /* global position setpoint valid, use it */ double sp_lat = global_pos_sp.lat * 1e-7; @@ -470,6 +464,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; att_sp.yaw_body = global_pos_sp.yaw; @@ -489,6 +484,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } } + reset_auto_pos = true; } @@ -496,9 +492,44 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) global_pos_sp_reproject = true; } - /* reset setpoints after non-manual modes */ + /* reset setpoints after AUTO mode */ reset_sp_xy = true; reset_sp_z = true; + + } else { + /* no control, loiter or stay on ground */ + if (local_pos.landed) { + /* landed: move setpoint down */ + /* in air: hold altitude */ + if (local_pos_sp.z < 5.0f) { + /* set altitude setpoint to 5m under ground, + * don't set it too deep to avoid unexpected landing in case of false "landed" signal */ + local_pos_sp.z = 5.0f; + mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z); + } + + reset_sp_z = true; + + } else { + /* in air: hold altitude */ + if (reset_sp_z) { + reset_sp_z = false; + local_pos_sp.z = local_pos.z; + mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z); + } + } + + if (control_mode.flag_control_position_enabled) { + if (reset_sp_xy) { + reset_sp_xy = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; + att_sp.yaw_body = att.yaw; + mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y); + } + } } /* publish local position setpoint */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index ef13ade88..4a4572b09 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index e15ddde26..093c6775d 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -80,9 +80,7 @@ struct vehicle_control_mode_s bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ - + bool flag_control_auto_enabled; // TEMP uint8_t auto_state; // TEMP navigation state for AUTO modes }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9f55bb874..43218eac4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -174,8 +174,6 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ -// uint64_t failsave_highlevel_begin; TO BE COMPLETED main_state_t main_state; /**< main state machine */ navigation_state_t navigation_state; /**< navigation state machine */ @@ -207,22 +205,13 @@ struct vehicle_status_s bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; - bool rc_signal_lost; /**< true if RC reception is terminally lost */ - bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ - uint64_t rc_signal_lost_interval; /**< interval in microseconds since when no RC signal is available */ + bool rc_signal_lost; /**< true if RC reception lost */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ - bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; - - bool preflight_calibration; - - bool system_emergency; - /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; -- cgit v1.2.3 From 55cfa5152c5e3ca3148eee776ec4467dd53eeca3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:41:59 +0200 Subject: Made low/critical warnings consisting --- src/drivers/blinkm/blinkm.cpp | 4 ++-- src/modules/commander/commander.cpp | 16 ++++++++-------- src/modules/uORB/topics/vehicle_status.h | 6 +++--- 3 files changed, 13 insertions(+), 13 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 490002254..2361f4dd1 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -541,7 +541,7 @@ BlinkM::led() printf(" cells found:%d\n", num_of_cells); } else { - if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* LED Pattern for battery low warning */ led_color_1 = LED_YELLOW; led_color_2 = LED_YELLOW; @@ -553,7 +553,7 @@ BlinkM::led() led_color_8 = LED_YELLOW; led_blink = LED_BLINK; - } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* LED Pattern for battery critical alerting */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a548f943e..e90300aff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -890,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; } @@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[]) if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); - status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; if (armed.armed) { @@ -1160,12 +1160,12 @@ int commander_thread_main(int argc, char *argv[]) if (tune_arm() == OK) arm_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ if (tune_critical_bat() == OK) battery_tune_played = true; @@ -1258,10 +1258,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { @@ -1272,10 +1272,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else if (armed->ready_to_arm) { for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) { + } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; } else { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 43218eac4..1c184d3a7 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -153,9 +153,9 @@ enum VEHICLE_TYPE { }; enum VEHICLE_BATTERY_WARNING { - VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ - VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ + VEHICLE_BATTERY_WARNING_LOW, /**< warning of low voltage */ + VEHICLE_BATTERY_WARNING_CRITICAL /**< alerting of critical voltage */ }; /** -- cgit v1.2.3 From 9bcfe49cff7b8fce6dfbeac7f8233e554672ebff Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 29 Aug 2013 16:45:33 +0200 Subject: Changed RGBLED behaviour, breathe mode when disarmed and ready to arm --- src/drivers/drv_rgbled.h | 2 + src/drivers/rgbled/rgbled.cpp | 46 ++++++++++++++++-- src/modules/commander/commander.cpp | 93 ++++++++++++++----------------------- 3 files changed, 77 insertions(+), 64 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h index 3c8bdec5d..07c6186dd 100644 --- a/src/drivers/drv_rgbled.h +++ b/src/drivers/drv_rgbled.h @@ -78,6 +78,7 @@ /** set pattern */ #define RGBLED_SET_PATTERN _RGBLEDIOC(7) + /* structure passed to RGBLED_SET_RGB ioctl() Note that the driver scales the brightness to 0 to 255, regardless @@ -115,6 +116,7 @@ typedef enum { RGBLED_MODE_BLINK_SLOW, RGBLED_MODE_BLINK_NORMAL, RGBLED_MODE_BLINK_FAST, + RGBLED_MODE_BREATHE, RGBLED_MODE_PATTERN } rgbled_mode_t; diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 05f079ede..feb8f1c6c 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012, 2013 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 @@ -96,6 +96,11 @@ private: rgbled_mode_t _mode; rgbled_pattern_t _pattern; + float _brightness; + uint8_t _r; + uint8_t _g; + uint8_t _b; + bool _should_run; bool _running; int _led_interval; @@ -104,6 +109,7 @@ private: void set_color(rgbled_color_t ledcolor); void set_mode(rgbled_mode_t mode); void set_pattern(rgbled_pattern_t *pattern); + void set_brightness(float brightness); static void led_trampoline(void *arg); void led(); @@ -128,6 +134,10 @@ RGBLED::RGBLED(int bus, int rgbled) : _color(RGBLED_COLOR_OFF), _mode(RGBLED_MODE_OFF), _running(false), + _brightness(1.0f), + _r(0), + _g(0), + _b(0), _led_interval(0), _counter(0) { @@ -191,7 +201,6 @@ int RGBLED::ioctl(struct file *filp, int cmd, unsigned long arg) { int ret = ENOTTY; - switch (cmd) { case RGBLED_SET_RGB: /* set the specified RGB values */ @@ -246,6 +255,15 @@ RGBLED::led() else set_on(false); break; + case RGBLED_MODE_BREATHE: + if (_counter >= 30) + _counter = 0; + if (_counter <= 15) { + set_brightness(((float)_counter)*((float)_counter)/(15.0f*15.0f)); + } else { + set_brightness(((float)(30-_counter))*((float)(30-_counter))/(15.0f*15.0f)); + } + break; case RGBLED_MODE_PATTERN: /* don't run out of the pattern array and stop if the next frame is 0 */ if (_counter >= RGBLED_PATTERN_LENGTH || _pattern.duration[_counter] <= 0) @@ -294,7 +312,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_AMBER: // amber set_rgb(255,20,0); break; - case RGBLED_COLOR_DIM_RED: // red + case RGBLED_COLOR_DIM_RED: // red set_rgb(90,0,0); break; case RGBLED_COLOR_DIM_YELLOW: // yellow @@ -306,7 +324,7 @@ RGBLED::set_color(rgbled_color_t color) { case RGBLED_COLOR_DIM_GREEN: // green set_rgb(0,90,0); break; - case RGBLED_COLOR_DIM_BLUE: // blue + case RGBLED_COLOR_DIM_BLUE: // blue set_rgb(0,0,90); break; case RGBLED_COLOR_DIM_WHITE: // white @@ -347,6 +365,12 @@ RGBLED::set_mode(rgbled_mode_t mode) _should_run = true; _led_interval = 100; break; + case RGBLED_MODE_BREATHE: + _should_run = true; + set_on(true); + _counter = 0; + _led_interval = 1000/15; + break; case RGBLED_MODE_PATTERN: _should_run = true; set_on(true); @@ -377,6 +401,13 @@ RGBLED::set_pattern(rgbled_pattern_t *pattern) set_mode(RGBLED_MODE_PATTERN); } +void +RGBLED::set_brightness(float brightness) { + + _brightness = brightness; + set_rgb(_r, _g, _b); +} + int RGBLED::set(bool on, uint8_t r, uint8_t g, uint8_t b) { @@ -413,7 +444,12 @@ RGBLED::set_on(bool on) int RGBLED::set_rgb(uint8_t r, uint8_t g, uint8_t b) { - const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)(b*15/255), SUB_ADDR_PWM1, (uint8_t)(g*15/255), SUB_ADDR_PWM2, (uint8_t)(r*15/255)}; + /* save the RGB values in case we want to change the brightness later */ + _r = r; + _g = g; + _b = b; + + const uint8_t msg[6] = { SUB_ADDR_PWM0, (uint8_t)((float)b/255.0f*15.0f*_brightness), SUB_ADDR_PWM1, (uint8_t)((float)g/255.0f*15.0f*_brightness), SUB_ADDR_PWM2, (uint8_t)((float)r/255.0f*15.0f*_brightness)}; return transfer(msg, sizeof(msg), nullptr, 0); } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e90300aff..776cd5766 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -689,6 +689,8 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + toggle_status_leds(&status, &armed, true); + /* now initialized */ commander_initialized = true; thread_running = true; @@ -1252,72 +1254,45 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - int i; - rgbled_pattern_t pattern; - memset(&pattern, 0, sizeof(pattern)); + /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { - /* armed, solid */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[0] = 1000; - + rgbled_set_mode(RGBLED_MODE_ON); } else if (armed->ready_to_arm) { - for (i = 0; i < 3; i++) { - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER; - - } else if (status->battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - - } else { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN; - } - - pattern.duration[i * 2] = 200; - - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; - } - - if (status->condition_global_position_valid) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 1000; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 800; + rgbled_set_mode(RGBLED_MODE_BREATHE); + } else { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + } - } else { - for (i = 3; i < 6; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE; - pattern.duration[i * 2] = 100; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 100; - } - pattern.color[6 * 2] = RGBLED_COLOR_OFF; - pattern.duration[6 * 2] = 700; - } - - } else { - for (i = 0; i < 3; i++) { - pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED; - pattern.duration[i * 2] = 200; - pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF; - pattern.duration[i * 2 + 1] = 200; - } + } - /* not ready to arm, blink at 10Hz */ + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + switch (status->battery_warning) { + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + default: + break; + } + } else { + switch (status->main_state) { + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + default: + break; } - - rgbled_set_pattern(&pattern); } /* give system warnings on error LED, XXX maybe add memory usage warning too */ -- cgit v1.2.3 From 3a00def1899223514ea0f9bd6bd567ac11d02f7e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 Aug 2013 10:11:24 +0200 Subject: commander: switch to AUTO_READY or AUTO_MISSION immediately, don't try to stay on ground --- src/modules/commander/commander.cpp | 84 +++++++++++++------------- src/modules/commander/commander_params.c | 2 +- src/modules/commander/state_machine_helper.cpp | 23 +++---- 3 files changed, 52 insertions(+), 57 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 776cd5766..8ddd86d03 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,7 @@ static uint64_t last_print_mode_reject_time = 0; /* if connected via USB */ static bool on_usb_power = false; +static float takeoff_alt = 5.0f; /* tasks waiting for low prio thread */ typedef enum { @@ -492,9 +493,10 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); param_t _param_component_id = param_find("MAV_COMP_ID"); + param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); /* welcome user */ - warnx("[commander] starting"); + warnx("starting"); /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -733,8 +735,11 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_component_id, &(status.component_id)); status_changed = true; - /* Re-check RC calibration */ + /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check()); + + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); } } @@ -1253,7 +1258,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ if (armed->armed) { @@ -1263,8 +1267,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang } else { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } - - } if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { @@ -1455,54 +1457,52 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (status->main_state == MAIN_STATE_AUTO) { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + // TODO AUTO_LAND handling if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ - if (local_pos->z > -5.0f || status->condition_landed) { - res = TRANSITION_NOT_CHANGED; + if (local_pos->z > -takeoff_alt || status->condition_landed) { + return TRANSITION_NOT_CHANGED; } } - - if (res != TRANSITION_NOT_CHANGED) { - /* check again, state can be changed */ - if (status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + /* possibly on ground, switch to TAKEOFF if needed */ + if (local_pos->z > -takeoff_alt || status->condition_landed) { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + return res; + } + } + /* switch to AUTO mode */ + if (status->rc_signal_found_once && !status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* not landed */ - if (status->rc_signal_found_once && !status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + if (status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - /* switch to MISSION in air when no RC control */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - res = TRANSITION_NOT_CHANGED; - - } else { - res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - } + /* LOITER */ + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - } + } else { + /* switch to MISSION when no RC control and first time in some AUTO mode */ + if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + res = TRANSITION_NOT_CHANGED; + } else { + res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + } + } } else { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 0a1703b2e..f22dac0c1 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,7 +45,7 @@ #include #include -PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 674f3feda..3cef10185 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -348,20 +348,15 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t break; case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = false; - control_mode->flag_control_auto_enabled = true; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_auto_enabled = true; break; case NAVIGATION_STATE_AUTO_LOITER: -- cgit v1.2.3 From 3f4315b4767ff221936e135b3252794a952f2b95 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Aug 2013 11:49:36 +0200 Subject: Hotfix: Increase stack size for low prio commander task --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8ddd86d03..45e0307e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -594,7 +594,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); - pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; /* low priority */ -- cgit v1.2.3 From f6bf1c7bf2d83bfc70fb8c4b9f589b25bcccc5e4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Sep 2013 10:29:30 +0200 Subject: Closing files that should be closed --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45e0307e6..8e8226f09 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -319,6 +319,8 @@ void print_status() break; } + close(state_sub); + warnx("arming: %s", armed_str); } @@ -1764,5 +1766,7 @@ void *commander_low_prio_loop(void *arg) } + close(cmd_sub); + return 0; } -- cgit v1.2.3 From aa785b0d2b339a8fcc730e11b63264b1ff8d146d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Sep 2013 13:24:21 +0200 Subject: Hotfix: Better error reporting, fixed sched param setup --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++++---- src/modules/commander/rc_calibration.cpp | 14 ++++++++------ src/modules/systemlib/param/param.c | 4 ++-- 3 files changed, 31 insertions(+), 12 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8e8226f09..333fe30ae 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -599,6 +600,7 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; + (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); @@ -1655,13 +1657,13 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 1) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot */ systemreset(false); } else if (((int)(cmd.param1)) == 3) { answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - usleep(1000000); + usleep(100000); /* reboot to bootloader */ systemreset(true); @@ -1704,6 +1706,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ @@ -1729,22 +1732,36 @@ void *commander_low_prio_loop(void *arg) case VEHICLE_CMD_PREFLIGHT_STORAGE: { if (((int)(cmd.param1)) == 0) { - if (0 == param_load_default()) { + int ret = param_load_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { - if (0 == param_save_default()) { + int ret = param_save_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + /* convenience as many parts of NuttX use negative errno */ + if (ret < 0) + ret = -ret; + + if (ret < 1000) + mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 9fc1d6470..90ede499a 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -57,14 +57,16 @@ int do_rc_calibration(int mavlink_fd) { mavlink_log_info(mavlink_fd, "trim calibration starting"); - /* XXX fix this */ - // if (current_status.rc_signal) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; + bool changed; + orb_check(sub_man, &changed); + + if (!changed) { + mavlink_log_critical(mavlink_fd, "no manual control, aborting"); + return ERROR; + } + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 24b52d1a9..c69de52b7 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -520,7 +520,7 @@ param_save_default(void) if (fd < 0) { warn("opening '%s' for writing failed", param_get_default_file()); - return -1; + return fd; } result = param_export(fd, false); @@ -529,7 +529,7 @@ param_save_default(void) if (result != 0) { warn("error exporting parameters to '%s'", param_get_default_file()); unlink(param_get_default_file()); - return -2; + return result; } return 0; -- cgit v1.2.3 From 56a35cc8896b077e70226541a43aa0d449e8d9bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 20:05:15 +0200 Subject: Fixed commander start / stop to ensure the state is sane once NSH returns --- src/modules/commander/commander.cpp | 45 ++++++++++++++++++++++++++++--------- 1 file changed, 34 insertions(+), 11 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 333fe30ae..830ee9743 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -144,8 +144,8 @@ static int mavlink_fd; /* flags */ static bool commander_initialized = false; -static bool thread_should_exit = false; /**< daemon exit flag */ -static bool thread_running = false; /**< daemon status flag */ +static volatile bool thread_should_exit = false; /**< daemon exit flag */ +static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static unsigned int leds_counter; @@ -230,7 +230,7 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - warnx("commander already running\n"); + warnx("commander already running"); /* this is not an error */ exit(0); } @@ -242,21 +242,38 @@ int commander_main(int argc, char *argv[]) 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); + + while (!thread_running) { + usleep(200); + } + exit(0); } if (!strcmp(argv[1], "stop")) { + + if (!thread_running) + errx(0, "commander already stopped"); + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - warnx("\tcommander is running\n"); + warnx("\tcommander is running"); print_status(); } else { - warnx("\tcommander not started\n"); + warnx("\tcommander not started"); } exit(0); @@ -595,16 +612,20 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] started"); + int ret; + pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); pthread_attr_setstacksize(&commander_low_prio_attr, 2992); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); + /* low priority */ param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + pthread_attr_destroy(&commander_low_prio_attr); /* Start monitoring loop */ unsigned counter = 0; @@ -1200,7 +1221,12 @@ int commander_thread_main(int argc, char *argv[]) } /* wait for threads to complete */ - pthread_join(commander_low_prio_thread, NULL); + ret = pthread_join(commander_low_prio_thread, NULL); + if (ret) { + warn("join failed", ret); + } + + rgbled_set_mode(RGBLED_MODE_OFF); /* close fds */ led_deinit(); @@ -1218,9 +1244,6 @@ int commander_thread_main(int argc, char *argv[]) close(param_changed_sub); close(battery_sub); - warnx("exiting"); - fflush(stdout); - thread_running = false; return 0; @@ -1628,7 +1651,7 @@ void *commander_low_prio_loop(void *arg) while (!thread_should_exit) { /* wait for up to 100ms for data */ - int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000); + int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) @@ -1785,5 +1808,5 @@ void *commander_low_prio_loop(void *arg) close(cmd_sub); - return 0; + return NULL; } -- cgit v1.2.3 From d3ac8c9ff31ac609d555613e552b38782a2b2490 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Sep 2013 21:06:55 +0200 Subject: Fixed HIL mode switching, HIL works --- src/modules/commander/commander.cpp | 10 ++++++++-- src/modules/commander/state_machine_helper.cpp | 2 ++ 2 files changed, 10 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 830ee9743..5eeb018fd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -343,6 +343,9 @@ void print_status() warnx("arming: %s", armed_str); } +static orb_advert_t control_mode_pub; +static orb_advert_t status_pub; + void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ @@ -356,6 +359,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ @@ -543,7 +550,6 @@ int commander_thread_main(int argc, char *argv[]) } /* Main state machine */ - orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value @@ -555,7 +561,7 @@ int commander_thread_main(int argc, char *argv[]) /* flags for control apps */ struct vehicle_control_mode_s control_mode; - orb_advert_t control_mode_pub; + /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cef10185..7d759b8d2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -502,6 +502,8 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + // XXX also set lockdown here + ret = OK; } else { -- cgit v1.2.3 From 8a70efdf43706b42e552a131e332789c114c7d4b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Sep 2013 08:55:45 +0200 Subject: Beep and print message when arming is not allowed --- src/modules/commander/commander.cpp | 104 ++++++++++++++++++++++-------------- 1 file changed, 65 insertions(+), 39 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5eeb018fd..a2443b0f6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,7 +369,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + } if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); @@ -561,7 +567,7 @@ int commander_thread_main(int argc, char *argv[]) /* flags for control apps */ struct vehicle_control_mode_s control_mode; - + /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -1083,10 +1089,18 @@ int commander_thread_main(int argc, char *argv[]) /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ if (status.arming_state == ARMING_STATE_STANDBY && - status.main_state == MAIN_STATE_MANUAL && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + + } else { + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + } + stick_on_counter = 0; } else { @@ -1106,15 +1120,8 @@ int commander_thread_main(int argc, char *argv[]) } } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates safety switch not pressed */ - - if (!(!safety.safety_switch_available || safety.safety_off)) { - print_reject_arm("NOT ARMING: Press safety switch first."); - - } else { - warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - } + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } /* fill current_status according to mode switches */ @@ -1228,6 +1235,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ ret = pthread_join(commander_low_prio_thread, NULL); + if (ret) { warn("join failed", ret); } @@ -1295,8 +1303,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (armed->armed) { rgbled_set_mode(RGBLED_MODE_ON); + } else if (armed->ready_to_arm) { rgbled_set_mode(RGBLED_MODE_BREATHE); + } else { rgbled_set_mode(RGBLED_MODE_BLINK_FAST); } @@ -1304,29 +1314,35 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - default: - break; + case VEHICLE_BATTERY_WARNING_LOW: + rgbled_set_color(RGBLED_COLOR_YELLOW); + break; + + case VEHICLE_BATTERY_WARNING_CRITICAL: + rgbled_set_color(RGBLED_COLOR_AMBER); + break; + + default: + break; } + } else { switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - default: - break; + case MAIN_STATE_MANUAL: + rgbled_set_color(RGBLED_COLOR_WHITE); + break; + + case MAIN_STATE_SEATBELT: + case MAIN_STATE_EASY: + rgbled_set_color(RGBLED_COLOR_GREEN); + break; + + case MAIN_STATE_AUTO: + rgbled_set_color(RGBLED_COLOR_BLUE); + break; + + default: + break; } } @@ -1497,16 +1513,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c return TRANSITION_NOT_CHANGED; } } + if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF && - status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && - status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && - status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { + status->navigation_state != NAVIGATION_STATE_AUTO_LOITER && + status->navigation_state != NAVIGATION_STATE_AUTO_MISSION && + status->navigation_state != NAVIGATION_STATE_AUTO_RTL) { /* possibly on ground, switch to TAKEOFF if needed */ if (local_pos->z > -takeoff_alt || status->condition_landed) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } } + /* switch to AUTO mode */ if (status->rc_signal_found_once && !status->rc_signal_lost) { /* act depending on switches when manual control enabled */ @@ -1524,18 +1542,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } + } else { /* switch to MISSION when no RC control and first time in some AUTO mode */ if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER || - status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || - status->navigation_state == NAVIGATION_STATE_AUTO_RTL || - status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { + status->navigation_state == NAVIGATION_STATE_AUTO_MISSION || + status->navigation_state == NAVIGATION_STATE_AUTO_RTL || + status->navigation_state == NAVIGATION_STATE_AUTO_LAND) { res = TRANSITION_NOT_CHANGED; } else { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } + } else { /* disarmed, always switch to AUTO_READY */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1762,35 +1782,41 @@ void *commander_low_prio_loop(void *arg) if (((int)(cmd.param1)) == 0) { int ret = param_load_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } else if (((int)(cmd.param1)) == 1) { int ret = param_save_default(); + if (ret == OK) { mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } } -- cgit v1.2.3 From 2362041dc127620ac1fb823b8aab8416ba17f0c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Sep 2013 19:58:41 +0200 Subject: commander: state indication changed, 3 green blinks = succsess, 3 white blinks = neutral, 3 red blinks = reject --- src/modules/commander/commander.cpp | 119 ++++++++++++++--------------- src/modules/commander/commander_helper.cpp | 50 +++++++++--- src/modules/commander/commander_helper.h | 3 +- 3 files changed, 100 insertions(+), 72 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a2443b0f6..1e86e8e24 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -5,6 +5,7 @@ * Lorenz Meier * Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -650,7 +651,6 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; - uint64_t start_time = 0; bool status_changed = true; @@ -728,7 +728,7 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - toggle_status_leds(&status, &armed, true); + control_status_leds(&status, &armed, true); /* now initialized */ commander_initialized = true; @@ -950,11 +950,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - // XXX not sure what should happen when voltage is low in flight - //arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - // XXX should we still allow to arm with critical battery? - //arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -1166,9 +1164,6 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; - - } else { - status_changed = false; } hrt_abstime t1 = hrt_absolute_time(); @@ -1228,7 +1223,19 @@ int commander_thread_main(int argc, char *argv[]) fflush(stdout); counter++; - toggle_status_leds(&status, &armed, arming_state_changed || status_changed); + int blink_state = blink_msg_state(); + if (blink_state > 0) { + /* blinking LED message, don't touch LEDs */ + if (blink_state == 2) { + /* blinking LED message completed, restore normal state */ + control_status_leds(&status, &armed, true); + } + } else { + /* normal state */ + control_status_leds(&status, &armed, status_changed); + } + + status_changed = false; usleep(COMMANDER_MONITORING_INTERVAL); } @@ -1276,8 +1283,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { + /* driving rgbled */ + if (changed) { + bool set_normal_color = false; + /* set mode */ + if (status->arming_state == ARMING_STATE_ARMED) { + rgbled_set_mode(RGBLED_MODE_ON); + set_normal_color = true; + + } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + rgbled_set_color(RGBLED_COLOR_RED); + + } else if (status->arming_state == ARMING_STATE_STANDBY) { + rgbled_set_mode(RGBLED_MODE_BREATHE); + set_normal_color = true; + + } else { // STANDBY_ERROR and other states + rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL); + rgbled_set_color(RGBLED_COLOR_RED); + } + + if (set_normal_color) { + /* set color */ + if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { + rgbled_set_color(RGBLED_COLOR_AMBER); + } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ + + } else { + if (status->condition_local_position_valid) { + rgbled_set_color(RGBLED_COLOR_GREEN); + + } else { + rgbled_set_color(RGBLED_COLOR_BLUE); + } + } + } + } + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ @@ -1298,54 +1345,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang #endif - if (changed) { - /* XXX TODO blink fast when armed and serious error occurs */ - - if (armed->armed) { - rgbled_set_mode(RGBLED_MODE_ON); - - } else if (armed->ready_to_arm) { - rgbled_set_mode(RGBLED_MODE_BREATHE); - - } else { - rgbled_set_mode(RGBLED_MODE_BLINK_FAST); - } - } - - if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) { - switch (status->battery_warning) { - case VEHICLE_BATTERY_WARNING_LOW: - rgbled_set_color(RGBLED_COLOR_YELLOW); - break; - - case VEHICLE_BATTERY_WARNING_CRITICAL: - rgbled_set_color(RGBLED_COLOR_AMBER); - break; - - default: - break; - } - - } else { - switch (status->main_state) { - case MAIN_STATE_MANUAL: - rgbled_set_color(RGBLED_COLOR_WHITE); - break; - - case MAIN_STATE_SEATBELT: - case MAIN_STATE_EASY: - rgbled_set_color(RGBLED_COLOR_GREEN); - break; - - case MAIN_STATE_AUTO: - rgbled_set_color(RGBLED_COLOR_BLUE); - break; - - default: - break; - } - } - /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { if (leds_counter % 2 == 0) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 017499cb5..565b4b66a 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -55,7 +56,6 @@ #include #include - #include "commander_helper.h" /* oddly, ERROR is not defined for c++ */ @@ -64,21 +64,24 @@ #endif static const int ERROR = -1; +#define BLINK_MSG_TIME 700000 // 3 fast blinks + bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } static int buzzer; +static hrt_abstime blink_msg_end; int buzzer_init() { @@ -104,16 +107,25 @@ void tune_error() void tune_positive() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE); } void tune_neutral() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_WHITE); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE); } void tune_negative() { + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_RED); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); } @@ -132,18 +144,31 @@ int tune_critical_bat() return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE); } - - void tune_stop() { ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); } +int blink_msg_state() +{ + if (blink_msg_end == 0) { + return 0; + + } else if (hrt_absolute_time() > blink_msg_end) { + return 2; + + } else { + return 1; + } +} + static int leds; static int rgbleds; int led_init() { + blink_msg_end = 0; + /* first open normal LEDs */ leds = open(LED_DEVICE_PATH, 0); @@ -159,6 +184,7 @@ int led_init() warnx("Blue LED: ioctl fail\n"); return ERROR; } + #endif if (ioctl(leds, LED_ON, LED_AMBER)) { @@ -168,6 +194,7 @@ int led_init() /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED_DEVICE_PATH, 0); + if (rgbleds == -1) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 errx(1, "Unable to open " RGBLED_DEVICE_PATH); @@ -203,19 +230,22 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } -void rgbled_set_color(rgbled_color_t color) { +void rgbled_set_color(rgbled_color_t color) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); } -void rgbled_set_mode(rgbled_mode_t mode) { +void rgbled_set_mode(rgbled_mode_t mode) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); } -void rgbled_set_pattern(rgbled_pattern_t *pattern) { +void rgbled_set_pattern(rgbled_pattern_t *pattern) +{ if (rgbleds != -1) ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 027d0535f..e9514446c 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -62,6 +62,7 @@ int tune_arm(void); int tune_low_bat(void); int tune_critical_bat(void); void tune_stop(void); +int blink_msg_state(); int led_init(void); void led_deinit(void); @@ -70,9 +71,7 @@ int led_on(int led); int led_off(int led); void rgbled_set_color(rgbled_color_t color); - void rgbled_set_mode(rgbled_mode_t mode); - void rgbled_set_pattern(rgbled_pattern_t *pattern); /** -- cgit v1.2.3 From 826d5687be209bc5e42ed97b8a84493913123c2a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 14:43:12 +0200 Subject: Fixed in-air restart --- src/modules/commander/commander.cpp | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e86e8e24..fd9067e90 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -470,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: - // XXX implement later - result = VEHICLE_CMD_RESULT_DENIED; + { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; + + } else { + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + } break; default: -- cgit v1.2.3 From 6616aa6f993c0dc767c7fe7b2e616202c79667d5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 14:58:06 +0200 Subject: Fixed in-air restart, now obeys the right order --- src/drivers/px4io/px4io.cpp | 3 +++ src/modules/commander/commander.cpp | 22 ++++++++++++---------- 2 files changed, 15 insertions(+), 10 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9597dad9a..952453a8c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -591,6 +591,9 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + /* get a status update from IO */ + io_get_status(); + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); log("INAIR RESTART RECOVERY (needs commander app running)"); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fd9067e90..01b7b84d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -824,16 +824,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); - orb_check(cmd_sub, &updated); - - if (updated) { - /* got command */ - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - - /* handle it */ - handle_command(&status, &safety, &control_mode, &cmd, &armed); - } - /* update safety topic */ orb_check(safety_sub, &updated); @@ -1165,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[]) } } + + /* handle commands last, as the system needs to be updated to handle them */ + orb_check(cmd_sub, &updated); + + if (updated) { + /* got command */ + orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); + + /* handle it */ + handle_command(&status, &safety, &control_mode, &cmd, &armed); + } + /* evaluate the navigation state machine */ transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position); -- cgit v1.2.3 From d1871bd7bb9ae9eefdf1ada56fc3f57428e689eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 6 Oct 2013 14:18:28 +0200 Subject: State machine fixes for HIL --- src/modules/commander/commander.cpp | 50 ++++++++++++++++---------- src/modules/commander/state_machine_helper.cpp | 29 +++++++++------ src/modules/commander/state_machine_helper.h | 2 +- src/modules/mavlink/waypoints.c | 4 ++- src/modules/sensors/sensors.cpp | 3 ++ 5 files changed, 56 insertions(+), 32 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d0..0c3546b95 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + /* reset the arming mode to disarmed */ + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { + mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); + } + } // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off) { + if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -532,6 +544,9 @@ static struct actuator_armed_s armed; static struct safety_s safety; +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - - /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -876,8 +887,8 @@ int commander_thread_main(int argc, char *argv[]) // warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { + /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[]) critical_voltage_counter++; } else { + low_voltage_counter = 0; critical_voltage_counter = 0; } @@ -978,7 +990,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1082,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1752,7 +1764,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1792,7 +1804,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f..efe12be57 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,9 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { + /* enforce lockdown in HIL */ + if (control_mode->flag_system_hil_enabled) { + armed->lockdown = true; + } else { + armed->lockdown = false; + } + switch (new_arming_state) { case ARMING_STATE_INIT: @@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED) { + || status->arming_state == ARMING_STATE_ARMED + || control_mode->flag_system_hil_enabled) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + valid_transition = false; break; case HIL_STATE_ON: if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + || current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1641b6f60..0bfdf36a8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 97472c233..f03fe4fdf 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -307,7 +307,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ { static uint16_t counter; - if (!global_pos->valid && !local_pos->xy_valid) { + if ((!global_pos->valid && !local_pos->xy_valid) || + /* no waypoint */ + wpm->size == 0) { /* nothing to check here, return */ return; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 085da905c..9ef0d3c83 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { + /* only read if publishing */ + if (!_publishing) + return; /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { -- cgit v1.2.3 From 3fd20481888fd9e63806f988153abe4bf82e7a04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Oct 2013 22:16:57 +0200 Subject: commander: remove duplicate publishing of vehicle_control_mode and actuator_armed topics, remove unused "counter" field from topics --- src/modules/commander/commander.cpp | 22 +++++----------------- src/modules/commander/state_machine_helper.cpp | 1 - src/modules/uORB/topics/vehicle_control_mode.h | 1 - 3 files changed, 5 insertions(+), 19 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d0..562790a7d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1181,31 +1181,19 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); - if (arming_state_changed || main_state_changed || navigation_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - status_changed = true; - } - hrt_abstime t1 = hrt_absolute_time(); - /* publish arming state */ - if (arming_state_changed) { - armed.timestamp = t1; - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); - } - - /* publish control mode */ if (navigation_state_changed || arming_state_changed) { - /* publish new navigation state */ control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - control_mode.counter++; - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { - status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); control_mode.timestamp = t1; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f..e4d235a6b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -497,7 +497,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (valid_transition) { current_status->hil_state = new_state; - current_status->counter++; current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d..38fb74c9b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,6 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; -- cgit v1.2.3 From ea89f23c917733f8a682c82e24e1e4f223f79843 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 21 Oct 2013 20:07:47 +0200 Subject: calibration: bugs fixed, mavlink messages cleanup --- src/drivers/drv_accel.h | 2 +- .../commander/accelerometer_calibration.cpp | 181 ++++++++++-------- src/modules/commander/commander.cpp | 54 +++--- src/modules/commander/gyro_calibration.cpp | 211 ++++++++++++--------- 4 files changed, 252 insertions(+), 196 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index eff5e7349..8a4f68428 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -66,7 +66,7 @@ struct accel_report { int16_t temperature_raw; }; -/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */ +/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ struct accel_scale { float x_offset; float x_scale; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 4880af907..d11d7eb5d 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -100,6 +100,24 @@ * accel_T = A^-1 * g * g = 9.80665 * + * ===== Rotation ===== + * + * Calibrating using model: + * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r) + * + * Actual correction: + * accel_corr = rot * accel_T * (accel_raw - accel_offs) + * + * Known: accel_T_r, accel_offs_r, rot + * Unknown: accel_T, accel_offs + * + * Solution: + * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs) + * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs) + * => accel_T = rot^-1 * accel_T_r * rot + * => accel_offs = rot^-1 * accel_offs_r + * * @author Anton Babushkin */ @@ -137,72 +155,97 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { - /* announce change */ - mavlink_log_info(mavlink_fd, "accel calibration started"); - mavlink_log_info(mavlink_fd, "accel cal progress <0> percent"); + mavlink_log_info(mavlink_fd, "accel calibration: started"); + mavlink_log_info(mavlink_fd, "accel calibration: progress <0>"); + + struct accel_scale accel_scale = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + int res = OK; + + /* reset all offsets to zero and all scales to one */ + int fd = open(ACCEL_DEVICE_PATH, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); + close(fd); + + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } /* measure and calculate offsets & scales */ float accel_offs[3]; float accel_T[3][3]; - int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); + res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T); if (res == OK) { - /* measurements complete successfully, rotate calibration values */ + /* measurements completed successfully, rotate calibration values */ param_t board_rotation_h = param_find("SENS_BOARD_ROT"); - enum Rotation board_rotation_id; - param_get(board_rotation_h, &(board_rotation_id)); + int32_t board_rotation_int; + param_get(board_rotation_h, &(board_rotation_int)); + enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; math::Matrix board_rotation(3, 3); get_rot_matrix(board_rotation_id, &board_rotation); - board_rotation = board_rotation.transpose(); + math::Matrix board_rotation_t = board_rotation.transpose(); math::Vector3 accel_offs_vec; accel_offs_vec.set(&accel_offs[0]); - math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec; + math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec; math::Matrix accel_T_mat(3, 3); accel_T_mat.set(&accel_T[0][0]); - math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation; + math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + + accel_scale.x_offset = accel_offs_rotated(0); + accel_scale.x_scale = accel_T_rotated(0, 0); + accel_scale.y_offset = accel_offs_rotated(1); + accel_scale.y_scale = accel_T_rotated(1, 1); + accel_scale.z_offset = accel_offs_rotated(2); + accel_scale.z_scale = accel_T_rotated(2, 2); /* set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0))) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1))) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2))) - || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0))) - || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1))) - || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) { - mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset)) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset)) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset)) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale)) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale)) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed"); + res = ERROR; } + } + if (res == OK) { + /* apply new scaling and offsets */ int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offs_rotated(0), - accel_T_rotated(0, 0), - accel_offs_rotated(1), - accel_T_rotated(1, 1), - accel_offs_rotated(2), - accel_T_rotated(2, 2), - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel"); + } + } + + if (res == OK) { /* auto-save to EEPROM */ - int save_ret = param_save_default(); + res = param_save_default(); - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); } + } - mavlink_log_info(mavlink_fd, "accel calibration done"); - return OK; + if (res == OK) { + mavlink_log_info(mavlink_fd, "accel calibration: done"); } else { - /* measurements error */ - mavlink_log_info(mavlink_fd, "accel calibration aborted"); - return ERROR; + mavlink_log_info(mavlink_fd, "accel calibration: failed"); } - /* exit accel calibration mode */ + return res; } int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) @@ -212,27 +255,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; - /* reset existing calibration */ - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); - close(fd); - - if (OK != ioctl_res) { - warn("ERROR: failed to set scale / offsets for accel"); - return ERROR; - } - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); unsigned done_count = 0; + int res = OK; while (true) { bool done = true; @@ -245,6 +271,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } + if (old_done_count != done_count) + mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count); + + if (done) + break; + mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", (!data_collected[1]) ? "x- " : "", @@ -253,17 +285,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (!data_collected[4]) ? "z+ " : "", (!data_collected[5]) ? "z- " : ""); - if (old_done_count != done_count) - mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count); - - if (done) - break; - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); if (orient < 0) { - close(sensor_combined_sub); - return ERROR; + res = ERROR; + break; } if (data_collected[orient]) { @@ -284,15 +310,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float close(sensor_combined_sub); - /* calculate offsets and transform matrix */ - int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res == OK) { + /* calculate offsets and transform matrix */ + res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); - if (res != 0) { - mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); - return ERROR; + if (res != OK) { + mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error"); + } } - return OK; + return res; } /* @@ -309,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; /* EMA time constant in seconds*/ - float ema_len = 0.2f; + float ema_len = 0.5f; /* set "still" threshold to 0.25 m/s^2 */ float still_thr2 = pow(0.25f, 2); /* set accel error threshold to 5m/s^2 */ @@ -342,8 +369,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) float w = dt / ema_len; for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; - float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; + float d = sensor.accelerometer_m_s2[i] - accel_ema[i]; + accel_ema[i] += d * w; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); @@ -389,8 +416,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) } if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor"); - return -1; + mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor"); + return ERROR; } } @@ -424,9 +451,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) return 5; // [ 0, 0, -g ] - mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); - return -2; // Can't detect orientation + return ERROR; // Can't detect orientation } /* diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980..9545ef171 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (hil_ret == OK && control_mode->flag_system_hil_enabled) { /* reset the arming mode to disarmed */ arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } @@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: - { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { - print_reject_arm("NOT ARMING: Press safety switch first."); - arming_res = TRANSITION_DENIED; + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { + transition_result_t arming_res = TRANSITION_NOT_CHANGED; - } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); - } + if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { + if (safety->safety_switch_available && !safety->safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); + arming_res = TRANSITION_DENIED; - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + } + + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } } - } break; default: @@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; /* check if board is connected via USB */ - struct stat statbuf; + //struct stat statbuf; //on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0); } @@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed) { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + } else { arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } @@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[]) counter++; int blink_state = blink_msg_state(); + if (blink_state > 0) { /* blinking LED message, don't touch LEDs */ if (blink_state == 2) { /* blinking LED message completed, restore normal state */ control_status_leds(&status, &armed, true); } + } else { /* normal state */ control_status_leds(&status, &armed, status_changed); @@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[]) ret = pthread_join(commander_low_prio_thread, NULL); if (ret) { - warn("join failed", ret); + warn("join failed: %d", ret); } rgbled_set_mode(RGBLED_MODE_OFF); @@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan /* driving rgbled */ if (changed) { bool set_normal_color = false; + /* set mode */ if (status->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); @@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) { rgbled_set_color(RGBLED_COLOR_AMBER); } + /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { @@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg) fds[0].events = POLLIN; while (!thread_should_exit) { - - /* wait for up to 100ms for data */ + /* wait for up to 200ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); - /* timed out - periodic check for _task_should_exit, etc. */ + /* timed out - periodic check for thread_should_exit, etc. */ if (pret == 0) continue; @@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ - answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_rc_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { @@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg) /* send acknowledge command */ // XXX TODO } - } close(cmd_sub); diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index e1d6e8340..219ae6cb2 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -58,7 +58,7 @@ static const int ERROR = -1; int do_gyro_calibration(int mavlink_fd) { - mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit."); + mavlink_log_info(mavlink_fd, "gyro calibration: started"); struct gyro_scale gyro_scale = { 0.0f, @@ -69,79 +69,85 @@ int do_gyro_calibration(int mavlink_fd) 1.0f, }; - /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); - struct gyro_report gyro_report; + int res = OK; /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to reset scale / offsets for gyro"); - + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets"); + } - /*** --- OFFSETS --- ***/ - - /* determine gyro mean values */ - const unsigned calibration_count = 5000; - unsigned calibration_counter = 0; - unsigned poll_errcount = 0; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_sensor_gyro; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); + /* subscribe to gyro sensor topic */ + int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro)); + struct gyro_report gyro_report; - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); - gyro_scale.x_offset += gyro_report.x; - gyro_scale.y_offset += gyro_report.y; - gyro_scale.z_offset += gyro_report.z; - calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) - mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count); - - } else { - poll_errcount++; + if (res == OK) { + /* determine gyro mean values */ + const unsigned calibration_count = 5000; + unsigned calibration_counter = 0; + unsigned poll_errcount = 0; + + while (calibration_counter < calibration_count) { + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = sub_sensor_gyro; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret > 0) { + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); + gyro_scale.x_offset += gyro_report.x; + gyro_scale.y_offset += gyro_report.y; + gyro_scale.z_offset += gyro_report.z; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count); + + } else { + poll_errcount++; + } + + if (poll_errcount > 1000) { + mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor"); + res = ERROR; + break; + } } - if (poll_errcount > 1000) { - mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor"); - close(sub_sensor_gyro); - return ERROR; - } + gyro_scale.x_offset /= calibration_count; + gyro_scale.y_offset /= calibration_count; + gyro_scale.z_offset /= calibration_count; } - gyro_scale.x_offset /= calibration_count; - gyro_scale.y_offset /= calibration_count; - gyro_scale.z_offset /= calibration_count; - - if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { - mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)"); - close(sub_sensor_gyro); - return ERROR; + if (res == OK) { + /* check offsets */ + if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) { + mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN"); + res = ERROR; + } } - /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro offset calibration done."); - tune_neutral(); - - /* set offset parameters to new values */ - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!"); + if (res == OK) { + /* set offset parameters to new values */ + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset)) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset)) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed"); + res = ERROR; + } } - - /*** --- SCALING --- ***/ #if 0 + /* beep on offset calibration end */ + mavlink_log_info(mavlink_fd, "gyro offset calibration done"); + tune_neutral(); + + /* scale calibration */ /* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); @@ -163,9 +169,11 @@ int do_gyro_calibration(int mavlink_fd) // XXX change to mag topic orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; - if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F; + + if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F; uint64_t last_time = hrt_absolute_time(); @@ -175,7 +183,7 @@ int do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) { + && (hrt_absolute_time() - start_time > 5 * 1e6)) { mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done"); close(sub_sensor_combined); return OK; @@ -203,14 +211,17 @@ int do_gyro_calibration(int mavlink_fd) // calculate error between estimate and measurement // apply declination correction for true heading as well. //float mag = -atan2f(magNav(1),magNav(0)); - float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); - if (mag > M_PI_F) mag -= 2*M_PI_F; - if (mag < -M_PI_F) mag += 2*M_PI_F; + float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]); + + if (mag > M_PI_F) mag -= 2 * M_PI_F; + + if (mag < -M_PI_F) mag += 2 * M_PI_F; float diff = mag - mag_last; - if (diff > M_PI_F) diff -= 2*M_PI_F; - if (diff < -M_PI_F) diff += 2*M_PI_F; + if (diff > M_PI_F) diff -= 2 * M_PI_F; + + if (diff < -M_PI_F) diff += 2 * M_PI_F; baseline_integral += diff; mag_last = mag; @@ -220,15 +231,15 @@ int do_gyro_calibration(int mavlink_fd) // warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral); - // } else if (poll_ret == 0) { - // /* any poll failure for 1s is a reason to abort */ - // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - // return; + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; } } float gyro_scale = baseline_integral / gyro_integral; - + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); @@ -236,42 +247,54 @@ int do_gyro_calibration(int mavlink_fd) if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) { mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)"); close(sub_sensor_gyro); + mavlink_log_critical(mavlink_fd, "gyro calibration failed"); return ERROR; } /* beep on calibration end */ - mavlink_log_info(mavlink_fd, "gyro scale calibration done."); + mavlink_log_info(mavlink_fd, "gyro scale calibration done"); tune_neutral(); #endif - /* set scale parameters to new values */ - if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) - || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) - || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { - mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!"); - } + close(sub_sensor_gyro); - /* apply new scaling and offsets */ - fd = open(GYRO_DEVICE_PATH, 0); + if (res == OK) { + /* set scale parameters to new values */ + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale)) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale)) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) { + mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed"); + res = ERROR; + } + } - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) - warn("WARNING: failed to apply new scale for gyro"); + if (res == OK) { + /* apply new scaling and offsets */ + fd = open(GYRO_DEVICE_PATH, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); + close(fd); - close(fd); + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro"); + } + } - /* auto-save to EEPROM */ - int save_ret = param_save_default(); + if (res == OK) { + /* auto-save to EEPROM */ + res = param_save_default(); - if (save_ret != 0) { - warnx("WARNING: auto-save of params to storage failed"); - mavlink_log_critical(mavlink_fd, "gyro store failed"); - close(sub_sensor_gyro); - return ERROR; + if (res != OK) { + mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters"); + } } - mavlink_log_info(mavlink_fd, "gyro calibration done."); + if (res == OK) { + mavlink_log_info(mavlink_fd, "gyro calibration: done"); - close(sub_sensor_gyro); - return OK; + } else { + mavlink_log_info(mavlink_fd, "gyro calibration: failed"); + } + + return res; } -- cgit v1.2.3 From 28b4e978534e164d08125a9b0cf1fe428d9ad122 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 22 Oct 2013 21:01:30 +0200 Subject: Fixed bug with fd leak in rc_calibration_check --- src/modules/commander/commander.cpp | 4 ++-- src/modules/systemlib/rc_check.c | 4 +--- src/modules/systemlib/rc_check.h | 2 +- src/systemcmds/preflight_check/preflight_check.c | 4 ++-- 4 files changed, 6 insertions(+), 8 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ef509980..db758c386 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -687,7 +687,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check()); + bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -802,7 +802,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check()); + rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c index 60d6473b8..b4350cc24 100644 --- a/src/modules/systemlib/rc_check.c +++ b/src/modules/systemlib/rc_check.c @@ -47,14 +47,12 @@ #include #include -int rc_calibration_check(void) { +int rc_calibration_check(int mavlink_fd) { char nbuf[20]; param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max, _parameter_handles_rev, _parameter_handles_dz; - int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - float param_min, param_max, param_trim, param_rev, param_dz; /* first check channel mappings */ diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h index e2238d151..e70b83cce 100644 --- a/src/modules/systemlib/rc_check.h +++ b/src/modules/systemlib/rc_check.h @@ -47,6 +47,6 @@ * @return 0 / OK if RC calibration is ok, index + 1 of the first * channel that failed else (so 1 == first channel failed) */ -__EXPORT int rc_calibration_check(void); +__EXPORT int rc_calibration_check(int mavlink_fd); __END_DECLS diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index e9c5f1a2c..1c58a2db6 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -140,7 +140,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- RC CALIBRATION ---- */ - bool rc_ok = (OK == rc_calibration_check()); + bool rc_ok = (OK == rc_calibration_check(mavlink_fd)); /* warn */ if (!rc_ok) @@ -227,4 +227,4 @@ static int led_off(int leds, int led) static int led_on(int leds, int led) { return ioctl(leds, LED_ON, led); -} \ No newline at end of file +} -- cgit v1.2.3 From 1336d625a83ba3f1afc207b64ec248dd1e15848a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Oct 2013 14:47:37 +0100 Subject: Hotfix: Announcing important messages via audio --- src/modules/commander/commander.cpp | 58 ++++++++++++++++++------------------- 1 file changed, 29 insertions(+), 29 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9814a7bcc..44a144296 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -379,7 +379,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); + //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ arming_res = TRANSITION_NOT_CHANGED; @@ -496,11 +496,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } @@ -521,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe tune_negative(); if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); } } @@ -874,10 +874,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] LANDED"); + mavlink_log_critical(mavlink_fd, "#audio: LANDED"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] IN AIR"); + mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } } @@ -955,7 +955,7 @@ int commander_thread_main(int argc, char *argv[]) //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; battery_tune_played = false; @@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[]) /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); + mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; battery_tune_played = false; @@ -1069,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[]) /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; } } @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } /* fill current_status according to mode switches */ @@ -1159,12 +1159,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1189,7 +1189,7 @@ int commander_thread_main(int argc, char *argv[]) if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } /* check which state machines for changes, clear "changed" flag */ @@ -1506,7 +1506,7 @@ print_reject_mode(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] WARNING: reject %s", msg); + sprintf(s, "#audio: warning: reject %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1520,7 +1520,7 @@ print_reject_arm(const char *msg) if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "[cmd] %s", msg); + sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); tune_negative(); } @@ -1617,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (res == TRANSITION_CHANGED) { if (control_mode->flag_control_position_enabled) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD"); } else { if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); } else { - mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD"); + mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); } } } @@ -1660,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); tune_negative(); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); tune_negative(); break; @@ -1814,14 +1814,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); + mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1834,14 +1834,14 @@ void *commander_low_prio_loop(void *arg) answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); } else { - mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); + mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ if (ret < 0) ret = -ret; if (ret < 1000) - mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret)); + mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } -- cgit v1.2.3 From 5781b58640a7bb3937f9eff979f99535ab1169e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Nov 2013 09:05:28 +0100 Subject: Minor bugfix to commander, emits arming sound now on the right occasions. Fixes an annoying issue where the arming sound would go off constantly if the safety was re-engaged in arming mode, something that we consider to be ok operationally --- src/modules/commander/commander.cpp | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 44a144296..ed090271c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -199,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); -void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed); +void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed); void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); @@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); + + // XXX this would be the right approach to do it, but do we *WANT* this? + // /* disarm if safety is now on and still armed */ + // if (safety.safety_switch_available && !safety.safety_off) { + // (void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + // } } /* update global position estimate */ @@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } @@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) +control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { @@ -1356,11 +1362,11 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ - if (armed->armed) { + if (actuator_armed->armed) { /* armed, solid */ led_on(LED_BLUE); - } else if (armed->ready_to_arm) { + } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ if (leds_counter % 20 == 0) led_toggle(LED_BLUE); -- cgit v1.2.3 From 4502c285eb8b284b7c08666b6d0e3e81035bace3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 5 Nov 2013 19:56:33 +0100 Subject: Startup scripts: Start the commander early and let it try to open the mavlink_fd with 20Hz --- ROMFS/px4fmu_common/init.d/rc.fixedwing | 5 ----- ROMFS/px4fmu_common/init.d/rc.multirotor | 5 ----- ROMFS/px4fmu_common/init.d/rcS | 5 +++++ src/modules/commander/commander.cpp | 17 +++++++---------- 4 files changed, 12 insertions(+), 20 deletions(-) (limited to 'src/modules/commander/commander.cpp') diff --git a/ROMFS/px4fmu_common/init.d/rc.fixedwing b/ROMFS/px4fmu_common/init.d/rc.fixedwing index 79e34a3b9..f02851006 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fixedwing +++ b/ROMFS/px4fmu_common/init.d/rc.fixedwing @@ -3,11 +3,6 @@ # Standard everything needed for fixedwing except mixer, actuator output and mavlink # -# -# Start the Commander -# -commander start - # # Start the sensors and test them. # diff --git a/ROMFS/px4fmu_common/init.d/rc.multirotor b/ROMFS/px4fmu_common/init.d/rc.multirotor index 6bae99175..bc550ac5a 100644 --- a/ROMFS/px4fmu_common/init.d/rc.multirotor +++ b/ROMFS/px4fmu_common/init.d/rc.multirotor @@ -3,11 +3,6 @@ # Standard everything needed for multirotors except mixer, actuator output and mavlink # -# -# Start the Commander -# -commander start - # # Start the sensors and test them. # diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cff8446a6..d8b5cb608 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -105,6 +105,11 @@ then blinkm systemstate fi fi + + # + # Start the Commander (needs to be this early for in-air-restarts) + # + commander start if param compare SYS_AUTOSTART 1000 then diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ed090271c..ace13bb78 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -116,6 +116,8 @@ extern struct system_load_s system_load; #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define MAVLINK_OPEN_INTERVAL 50000 + #define STICK_ON_OFF_LIMIT 0.75f #define STICK_THRUST_RANGE 1.0f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 @@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - if (mavlink_fd < 0) { - /* try again later */ - usleep(20000); - mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - - if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first."); - } - } - /* Main state machine */ /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); @@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[]) while (!thread_should_exit) { + if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { + /* try to open the mavlink log device every once in a while */ + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + /* update parameters */ orb_check(param_changed_sub, &updated); -- cgit v1.2.3