From c75c5a5f30cde02002d4db16d803dc979ce8d4d5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Nov 2013 23:11:14 +0100 Subject: Fixedwing: Enable loiter mode, tested in HIL --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ace13bb78..905d5dee5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1539,7 +1539,8 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c // 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 > -takeoff_alt || status->condition_landed) { + // XXX: only respect the condition_landed when the local position is actually valid + if (status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { return TRANSITION_NOT_CHANGED; } } -- cgit v1.2.3 From 668f9dc552040b77d298330ff2dc1dcccdb5b3da Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 8 Nov 2013 16:45:22 +0100 Subject: enable seatbelt without position lock for non rotary wing vehicles --- src/modules/commander/state_machine_helper.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6..a50af7daf 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -239,8 +239,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ - if (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid) { + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } -- cgit v1.2.3 From 22ef0c77352e9a60cecc51fc219dbad4b9bd9d0d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 10 Nov 2013 19:24:37 +0100 Subject: fix MEAS airspeed and airspeed calibration --- src/drivers/airspeed/airspeed.h | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 10 ++++-- src/modules/commander/airspeed_calibration.cpp | 44 +++++++++++++++++++++---- src/modules/sensors/sensors.cpp | 1 + src/modules/systemlib/airspeed.c | 2 +- src/modules/uORB/topics/differential_pressure.h | 4 +-- 7 files changed, 52 insertions(+), 13 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 048784813..62c0d1f17 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -119,7 +119,7 @@ protected: virtual int collect() = 0; work_s _work; - uint16_t _max_differential_pressure_pa; + float _max_differential_pressure_pa; bool _sensor_ok; int _measure_ticks; bool _collect_phase; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 9d8ad084e..de371bf32 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -180,7 +180,7 @@ ETSAirspeed::collect() differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = diff_pres_pa; + report.differential_pressure_pa = (float)diff_pres_pa; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b3003fc1b..3cd6d6720 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -36,6 +36,7 @@ * @author Lorenz Meier * @author Sarthak Kaingade * @author Simon Wilks + * @author Thomas Gubler * * Driver for the MEAS Spec series connected via I2C. * @@ -76,6 +77,7 @@ #include #include #include +#include #include #include @@ -184,7 +186,7 @@ MEASAirspeed::collect() //diff_pres_pa -= _diff_pres_offset; int16_t dp_raw = 0, dT_raw = 0; dp_raw = (val[0] << 8) + val[1]; - dp_raw = 0x3FFF & dp_raw; + dp_raw = 0x3FFF & dp_raw; //mask the used bits dT_raw = (val[2] << 8) + val[3]; dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200 * dT_raw) / 2047) - 50; @@ -193,7 +195,11 @@ MEASAirspeed::collect() // Calculate differential pressure. As its centered around 8000 // and can go positive or negative, enforce absolute value - uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); +// uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + const float P_min = -1.0f; + const float P_max = 1.0f; + float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset); + struct differential_pressure_s report; // Track maximum differential pressure measured (so we can work out top speed). diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 248eb4a66..1809f9688 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -37,12 +37,15 @@ */ #include "airspeed_calibration.h" +#include "calibration_messages.h" #include "commander_helper.h" #include +#include #include #include #include +#include #include #include #include @@ -55,10 +58,13 @@ #endif static const int ERROR = -1; +static const char *sensor_name = "dpress"; + int do_airspeed_calibration(int mavlink_fd) { /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); + mavlink_log_info(mavlink_fd, "don't move system"); const int calibration_count = 2500; @@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd) int calibration_counter = 0; float diff_pres_offset = 0.0f; + /* Reset sensor parameters */ + struct airspeed_scale airscale = { + 0.0f, + 1.0f, + }; + + bool paramreset_successful = false; + int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { + if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + paramreset_successful = true; + } + close(fd); + } + + if (!paramreset_successful) { + warn("WARNING: failed to set scale / offsets for airspeed sensor"); + mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); + return ERROR; + } + while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_pa; calibration_counter++; + if (calibration_counter % (calibration_count / 20) == 0) + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } @@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); return ERROR; } @@ -105,17 +136,18 @@ int 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"); + mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); close(diff_pres_sub); return ERROR; } - mavlink_log_info(mavlink_fd, "airspeed calibration done"); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(); close(diff_pres_sub); return OK; } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index da0c11372..6d38b98ec 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1121,6 +1121,7 @@ Sensors::parameter_update_poll(bool forced) if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) warn("WARNING: failed to set scale / offsets for airspeed sensor"); + close(fd); } #if 0 diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c index 310fbf60f..1a479c205 100644 --- a/src/modules/systemlib/airspeed.c +++ b/src/modules/systemlib/airspeed.c @@ -59,7 +59,7 @@ float calc_indicated_airspeed(float differential_pressure) { - if (differential_pressure > 0) { + if (differential_pressure > 0.0f) { return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); } else { return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index e4d2c92ce..5d94d4288 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,8 +54,8 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - uint16_t differential_pressure_pa; /**< Differential pressure reading */ - uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor */ -- cgit v1.2.3 From a94ed67b3fefa5437d0322948190c02d69f82fea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Nov 2013 13:47:37 +0100 Subject: update the commander to only use local pos for landing detection when on rotary wing --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 905d5dee5..dfd4d2f73 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[]) 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.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1540,7 +1540,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't switch to other states until takeoff not completed */ // XXX: only respect the condition_landed when the local position is actually valid - if (status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { return TRANSITION_NOT_CHANGED; } } @@ -1550,7 +1550,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c 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) { + if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); return res; } -- cgit v1.2.3 From 278e05e425f6aca75e2d6b43a17945b095176997 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Dec 2013 16:52:41 +0100 Subject: added simple flight termination state machine which enbales parachute on request --- src/modules/commander/commander.cpp | 19 +++++++- src/modules/commander/state_machine_helper.cpp | 51 ++++++++++++++++++++++ src/modules/commander/state_machine_helper.h | 4 ++ src/modules/fw_att_control/fw_att_control_main.cpp | 44 +++++++++++++++---- src/modules/uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 8 ++++ 6 files changed, 117 insertions(+), 10 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfd4d2f73..40562a4e1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -509,6 +509,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + /* Flight termination */ + case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command + + if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* reject parachute depoyment not armed */ + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } + break; + default: break; } @@ -1199,6 +1214,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = check_arming_state_changed(); bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); + bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); @@ -1725,7 +1741,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_NAV_TAKEOFF) + cmd.command == VEHICLE_CMD_NAV_TAKEOFF || + cmd.command == VEHICLE_CMD_DO_SET_SERVO) continue; /* only handle low-priority commands here */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index a50af7daf..6c21dfab0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -65,6 +65,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; static bool navigation_state_changed = true; +static bool flighttermination_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -451,6 +452,18 @@ check_navigation_state_changed() } } +bool +check_flighttermination_state_changed() +{ + if (flighttermination_state_changed) { + flighttermination_state_changed = false; + return true; + + } else { + return false; + } +} + void set_navigation_state_changed() { @@ -523,6 +536,44 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s } +/** +* Transition from one flightermination state to another +*/ +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_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_flighttermination_state == status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; +} + + // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0bfdf36a8..e1ec9d4ad 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -70,8 +70,12 @@ bool check_main_state_changed(); transition_result_t navigation_state_transition(struct vehicle_status_s *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode); + bool check_navigation_state_changed(); +bool check_flighttermination_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); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index ff3f13306..bb74534f0 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -121,6 +121,7 @@ private: orb_advert_t _rate_sp_pub; /**< rate setpoint publication */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint point */ orb_advert_t _actuators_0_pub; /**< actuator control group 0 setpoint */ + orb_advert_t _actuators_1_pub; /**< actuator control group 1 setpoint (Airframe) */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct accel_report _accel; /**< body frame accelerations */ @@ -128,7 +129,8 @@ private: struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _vcontrol_mode; /**< vehicle control mode */ - struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators; /**< actuator control inputs */ + struct actuator_controls_s _actuators_airframe; /**< actuator control inputs */ struct vehicle_global_position_s _global_pos; /**< global position */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -294,14 +296,15 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _airspeed_valid(false) { /* safely initialize structs */ - vehicle_attitude_s _att = {0}; - accel_report _accel = {0}; - vehicle_attitude_setpoint_s _att_sp = {0}; - manual_control_setpoint_s _manual = {0}; - airspeed_s _airspeed = {0}; - vehicle_control_mode_s _vcontrol_mode = {0}; - actuator_controls_s _actuators = {0}; - vehicle_global_position_s _global_pos = {0}; + _att = {0}; + _accel = {0}; + _att_sp = {0}; + _manual = {0}; + _airspeed = {0}; + _vcontrol_mode = {0}; + _actuators = {0}; + _actuators_airframe = {0}; + _global_pos = {0}; _parameter_handles.tconst = param_find("FW_ATT_TC"); @@ -625,6 +628,15 @@ FixedwingAttitudeControl::task_main() lock_integrator = true; } + /* Simple handling of failsafe: deploy parachute if failsafe is on */ + if (_vcontrol_mode.flag_control_flighttermination_enabled) { + _actuators_airframe.control[1] = 1.0f; +// warnx("_actuators_airframe.control[1] = 1.0f;"); + } else { + _actuators_airframe.control[1] = -1.0f; +// warnx("_actuators_airframe.control[1] = -1.0f;"); + } + /* decide if in stabilized or full manual control */ if (_vcontrol_mode.flag_control_attitude_enabled) { @@ -794,6 +806,7 @@ FixedwingAttitudeControl::task_main() /* lazily publish the setpoint only once available */ _actuators.timestamp = hrt_absolute_time(); + _actuators_airframe.timestamp = hrt_absolute_time(); if (_actuators_0_pub > 0) { /* publish the attitude setpoint */ @@ -804,6 +817,19 @@ FixedwingAttitudeControl::task_main() _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); } + if (_actuators_1_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_airframe); + warnx("%.2f %.2f %.2f %.2f %.2f %.2f %.2f %.2f", + (double)_actuators_airframe.control[0], (double)_actuators_airframe.control[1], (double)_actuators_airframe.control[2], + (double)_actuators_airframe.control[3], (double)_actuators_airframe.control[4], (double)_actuators_airframe.control[5], + (double)_actuators_airframe.control[6], (double)_actuators_airframe.control[7]); + + } else { + /* advertise and publish */ + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_airframe); + } + } perf_end(_loop_perf); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 38fb74c9b..e26fb97c8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -78,6 +78,7 @@ struct vehicle_control_mode_s 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 flag_control_flighttermination_enabled; /**< true if flighttermination 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 1c184d3a7..1c3763924 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -95,6 +95,12 @@ typedef enum { HIL_STATE_ON } hil_state_t; + +typedef enum { + FLIGHTTERMINATION_STATE_OFF = 0, + FLIGHTTERMINATION_STATE_ON +} flighttermination_state_t; + typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_ASSISTED, @@ -229,6 +235,8 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + flighttermination_state_t flighttermination_state; }; /** -- cgit v1.2.3 From 367d5d0cf28d6c857fdcd6c4ad20809f9e52e310 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Dec 2013 11:02:16 +0100 Subject: fix wrong usage of navigation state in flighttermination state machine --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6c21dfab0..ca3ec94b8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -544,7 +544,7 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->navigation_state) { + if (new_flighttermination_state == status->flighttermination_state) { ret = TRANSITION_NOT_CHANGED; } else { -- cgit v1.2.3 From 23d0c6f8dd1a0b9aa64dafe26cfd56e43637c5ee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 14 Dec 2013 11:03:02 +0100 Subject: temporary workaround to trigger failsafe with remote control --- src/modules/commander/commander.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 40562a4e1..9ed9051fa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1189,6 +1189,16 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ + if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode); + if (flighttermination_res == TRANSITION_CHANGED) { + tune_positive(); + } + } else { + flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode); + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); -- cgit v1.2.3 From ba612c3ee8b88b9352e7cfa723997887dd736b76 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 19 Dec 2013 14:10:25 +0400 Subject: mathlib fixes --- src/lib/conversion/rotation.cpp | 2 +- src/lib/conversion/rotation.h | 2 +- src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 43 ++- src/lib/ecl/l1/ecl_l1_pos_controller.h | 12 +- src/lib/external_lgpl/tecs/tecs.cpp | 6 +- src/lib/external_lgpl/tecs/tecs.h | 6 +- src/lib/mathlib/math/Matrix.hpp | 172 +++++++--- src/lib/mathlib/math/Matrix3.cpp | 46 --- src/lib/mathlib/math/Matrix3.hpp | 356 --------------------- src/lib/mathlib/math/Quaternion.hpp | 79 ++++- src/lib/mathlib/math/Vector.hpp | 224 ++++++++++--- src/lib/mathlib/math/Vector2.hpp | 269 ---------------- src/lib/mathlib/math/Vector3.hpp | 287 ----------------- src/lib/mathlib/mathlib.h | 3 - src/lib/mathlib/module.mk | 1 - src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 92 +++--- src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 20 +- .../commander/accelerometer_calibration.cpp | 14 +- src/modules/mavlink/mavlink_receiver.cpp | 11 +- src/modules/navigator/navigator_main.cpp | 16 +- src/modules/sensors/sensors.cpp | 14 +- src/systemcmds/tests/test_mathlib.cpp | 23 -- 22 files changed, 510 insertions(+), 1188 deletions(-) delete mode 100644 src/lib/mathlib/math/Matrix3.cpp delete mode 100644 src/lib/mathlib/math/Matrix3.hpp delete mode 100644 src/lib/mathlib/math/Vector2.hpp delete mode 100644 src/lib/mathlib/math/Vector3.hpp (limited to 'src/modules/commander') diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index bac594ab9..614877b18 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -41,7 +41,7 @@ #include "rotation.h" __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix) +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix) { float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll; float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch; diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h index 85c63c0fc..0c56494c5 100644 --- a/src/lib/conversion/rotation.h +++ b/src/lib/conversion/rotation.h @@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = { * Get the rotation matrix */ __EXPORT void -get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix); +get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix); #endif /* ROTATION_H_ */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 27d76f959..fb7ed486d 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) return _crosstrack_error; } -void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed_vector) { /* this follows the logic presented in [1] */ @@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float ltrack_vel; /* get the direction between the last (visited) and next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1)); /* enforce a minimum ground speed of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length(), 0.1f); @@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _L1_distance = _L1_ratio * ground_speed; /* calculate vector from A to B */ - math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B); + math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B); /* * check if waypoints are on top of each other. If yes, @@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c vector_AB.normalize(); /* calculate the vector from waypoint A to the aircraft */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* calculate crosstrack error (output only) */ _crosstrack_error = vector_AB % vector_A_to_airplane; @@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float alongTrackDist = vector_A_to_airplane * vector_AB; /* estimate airplane position WRT to B */ - math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); /* calculate angle of airplane position vector relative to line) */ @@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* calculate eta to fly to waypoint A */ /* unit vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); + math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized(); /* velocity across / orthogonal to line */ xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit); /* velocity along line */ ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); /* * If the AB vector and the vector from B to airplane point in the same @@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit); eta = atan2f(xtrack_vel, ltrack_vel); /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX()); + _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0)); } else { @@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float eta1 = asinf(sine_eta1); eta = eta1 + eta2; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1; } @@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c _bearing_error = eta; } -void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons float K_velocity = 2.0f * _L1_damping * omega; /* update bearing to next waypoint */ - _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY()); + _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1)); /* ground speed, enforce minimum of 0.1 m/s to avoid singularities */ float ground_speed = math::max(ground_speed_vector.length() , 0.1f); @@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons _L1_distance = _L1_ratio * ground_speed; /* calculate the vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); + math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position); /* store the normalized vector from waypoint A to current position */ - math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); + math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized(); /* calculate eta angle towards the loiter center */ @@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons /* angle between requested and current velocity vector */ _bearing_error = eta; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } else { _lateral_accel = lateral_accel_sp_circle; _circle_mode = true; _bearing_error = 0.0f; /* bearing from current position to L1 point */ - _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); + _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0)); } } -void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector) +void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector) { /* the complete guidance logic in this section was proposed by [2] */ @@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading) } -math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const +math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const { /* this is an approximation for small angles, proposed by [2] */ - math::Vector2f out; - - out.setX(math::radians((target.getX() - origin.getX()))); - out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX())))); + math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0))))); return out * static_cast(CONSTANTS_RADIUS_OF_EARTH); } diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index 7a3c42a92..5c0804a39 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -160,8 +160,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, - const math::Vector2f &ground_speed); + void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position, + const math::Vector<2> &ground_speed); /** @@ -172,8 +172,8 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction, - const math::Vector2f &ground_speed_vector); + void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction, + const math::Vector<2> &ground_speed_vector); /** @@ -185,7 +185,7 @@ public: * * @return sets _lateral_accel setpoint */ - void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed); + void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed); /** @@ -260,7 +260,7 @@ private: * @param wp The point to convert to into the local coordinates, in WGS84 coordinates * @return The vector in meters pointing from the reference position to the coordinates */ - math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const; + math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const; }; diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a733ef1d2..d7f350e5c 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -29,7 +29,7 @@ using namespace math; * */ -void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth) +void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth) { // Implement third order complementary filter for height and height rate // estimted height rate = _integ2_state @@ -257,7 +257,7 @@ void TECS::_update_energies(void) _SKEdot = _integ5_state * _vel_dot; } -void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat) +void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat) { // Calculate total energy values _STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est; @@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void) _STEdot_min = - _minSinkRate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, +void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index f8f832ed7..68832a039 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -71,10 +71,10 @@ public: // Update of the estimated height and height rate internal state // Update of the inertial speed rate internal state // Should be called at 50Hz or greater - void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth); + void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth); // Update the control loop calculations - void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, + void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); // demanded throttle in percentage @@ -338,7 +338,7 @@ private: void _update_energies(void); // Update Demanded Throttle - void _update_throttle(float throttle_cruise, const math::Dcm &rotMat); + void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat); // Detect Bad Descent void _detect_bad_descent(void); diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index 1849f58be..67214133a 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -42,16 +42,19 @@ #ifndef MATRIX_HPP #define MATRIX_HPP +#include #include "../CMSIS/Include/arm_math.h" namespace math { -template +template class Matrix; +class Quaternion; + // MxN matrix with float elements -template +template class MatrixBase { public: /** @@ -69,10 +72,14 @@ public: * note that this ctor will not initialize elements */ MatrixBase() { - //arm_mat_init_f32(&arm_mat, M, N, data); arm_mat = {M, N, &data[0][0]}; } + MatrixBase(const float *d) { + arm_mat = {M, N, &data[0][0]}; + memcpy(data, d, sizeof(data)); + } + /** * access by index */ @@ -98,10 +105,10 @@ public: /** * test for equality */ - bool operator ==(const MatrixBase &m) { + bool operator ==(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return false; return true; } @@ -109,10 +116,10 @@ public: /** * test for inequality */ - bool operator !=(const MatrixBase &m) { + bool operator !=(const Matrix &m) { for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - if (data[i][j] != m(i, j)) + if (data[i][j] != m.data[i][j]) return true; return false; } @@ -120,85 +127,97 @@ public: /** * set to value */ - const MatrixBase &operator =(const MatrixBase &m) { + const Matrix &operator =(const Matrix &m) { memcpy(data, m.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - MatrixBase operator -(void) const { - MatrixBase res; + Matrix operator -(void) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = -data[i][j]; + res.data[i][j] = -data[i][j]; return res; } /** * addition */ - MatrixBase operator +(const MatrixBase &m) const { - MatrixBase res; + Matrix operator +(const Matrix &m) const { + Matrix res; for (unsigned int i = 0; i < N; i++) for (unsigned int j = 0; j < M; j++) - res[i][j] = data[i][j] + m(i, j); + res.data[i][j] = data[i][j] + m.data[i][j]; return res; } - MatrixBase &operator +=(const MatrixBase &m) { - return *this = *this + m; + Matrix &operator +=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] += m.data[i][j]; + return *reinterpret_cast*>(this); } /** * subtraction */ - MatrixBase operator -(const MatrixBase &m) const { - MatrixBase res; + Matrix operator -(const Matrix &m) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] - m(i, j); + res.data[i][j] = data[i][j] - m.data[i][j]; return res; } - MatrixBase &operator -=(const MatrixBase &m) { - return *this = *this - m; + Matrix &operator -=(const Matrix &m) { + for (unsigned int i = 0; i < N; i++) + for (unsigned int j = 0; j < M; j++) + data[i][j] -= m.data[i][j]; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - MatrixBase operator *(const float num) const { - MatrixBase res; + Matrix operator *(const float num) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) - res[i][j] = data[i][j] * num; + res.data[i][j] = data[i][j] * num; return res; } - MatrixBase &operator *=(const float num) { - return *this = *this * num; + Matrix &operator *=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] *= num; + return *reinterpret_cast*>(this); } - MatrixBase operator /(const float num) const { - MatrixBase res; + Matrix operator /(const float num) const { + Matrix res; for (unsigned int i = 0; i < M; i++) for (unsigned int j = 0; j < N; j++) res[i][j] = data[i][j] / num; return res; } - MatrixBase &operator /=(const float num) { - return *this = *this / num; + Matrix &operator /=(const float num) { + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) + data[i][j] /= num; + return *this; } /** * multiplication by another matrix */ template - Matrix operator *(const Matrix &m) const { - Matrix res; + Matrix operator *(const Matrix &m) const { + Matrix res; arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); return res; } @@ -230,7 +249,7 @@ public: data[i][i] = 1; } - void dump(void) { + void print(void) { for (unsigned int i = 0; i < M; i++) { for (unsigned int j = 0; j < N; j++) printf("%.3f\t", data[i][j]); @@ -244,6 +263,12 @@ class Matrix : public MatrixBase { public: using MatrixBase::operator *; + Matrix() : MatrixBase() { + } + + Matrix(const float *d) : MatrixBase(d) { + } + /** * set to value */ @@ -255,18 +280,28 @@ public: /** * multiplication by a vector */ - Vector operator *(const Vector &v) const { + Vector operator *(const Vector &v) const { Vector res; arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col); return res; } }; +} + +#include "Quaternion.hpp" +namespace math { template <> class Matrix<3, 3> : public MatrixBase<3, 3> { public: using MatrixBase<3, 3>::operator *; + Matrix() : MatrixBase<3, 3>() { + } + + Matrix(const float *d) : MatrixBase<3, 3>(d) { + } + /** * set to value */ @@ -279,9 +314,70 @@ public: * multiplication by a vector */ Vector<3> operator *(const Vector<3> &v) const { - return Vector<3>(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], - data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], - data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2], + data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2], + data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]); + return res; + } + + /** + * create a rotation matrix from given quaternion + */ + void from_quaternion(const Quaternion &q) { + float aSq = q.data[0] * q.data[0]; + float bSq = q.data[1] * q.data[1]; + float cSq = q.data[2] * q.data[2]; + float dSq = q.data[3] * q.data[3]; + data[0][0] = aSq + bSq - cSq - dSq; + data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]); + data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]); + data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]); + data[1][1] = aSq - bSq + cSq - dSq; + data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]); + data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]); + data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]); + data[2][2] = aSq - bSq - cSq + dSq; + } + + /** + * create a rotation matrix from given euler angles + * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf + */ + void from_euler(float roll, float pitch, float yaw) { + float cp = cosf(pitch); + float sp = sinf(pitch); + float sr = sinf(roll); + float cr = cosf(roll); + float sy = sinf(yaw); + float cy = cosf(yaw); + + data[0][0] = cp * cy; + data[0][1] = (sr * sp * cy) - (cr * sy); + data[0][2] = (cr * sp * cy) + (sr * sy); + data[1][0] = cp * sy; + data[1][1] = (sr * sp * sy) + (cr * cy); + data[1][2] = (cr * sp * sy) - (sr * cy); + data[2][0] = -sp; + data[2][1] = sr * cp; + data[2][2] = cr * cp; + } + + Vector<3> to_euler(void) const { + Vector<3> euler; + euler.data[1] = asinf(-data[2][0]); + + if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0]; + + } else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) { + euler.data[0] = 0.0f; + euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0]; + + } else { + euler.data[0] = atan2f(data[2][1], data[2][2]); + euler.data[2] = atan2f(data[1][0], data[0][0]); + } } }; diff --git a/src/lib/mathlib/math/Matrix3.cpp b/src/lib/mathlib/math/Matrix3.cpp deleted file mode 100644 index 3f1259dc4..000000000 --- a/src/lib/mathlib/math/Matrix3.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Matrix3.cpp - * - * 3x3 Matrix - */ - -#include "Matrix3.hpp" - -namespace math -{ -} diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp deleted file mode 100644 index c7eb67ba7..000000000 --- a/src/lib/mathlib/math/Matrix3.hpp +++ /dev/null @@ -1,356 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Matrix3.hpp - * - * 3x3 Matrix - */ - -#ifndef MATRIX3_HPP -#define MATRIX3_HPP - -#include "Vector3.hpp" -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -// 3x3 matrix with elements of type T -template -class Matrix3 { -public: - /** - * matrix data[row][col] - */ - T data[3][3]; - - /** - * struct for using arm_math functions - */ - arm_matrix_instance_f32 arm_mat; - - /** - * trivial ctor - * note that this ctor will not initialize elements - */ - Matrix3() { - arm_mat = {3, 3, &data[0][0]}; - } - - /** - * setting ctor - */ - Matrix3(const T d[3][3]) { - arm_mat = {3, 3, &data[0][0]}; - memcpy(data, d, sizeof(data)); - } - - /** - * setting ctor - */ - Matrix3(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = ax; - data[0][1] = ay; - data[0][2] = az; - data[1][0] = bx; - data[1][1] = by; - data[1][2] = bz; - data[2][0] = cx; - data[2][1] = cy; - data[2][2] = cz; - } - - /** - * casting from a vector3f to a matrix is the tilde operator - */ - Matrix3(const Vector3 &v) { - arm_mat = {3, 3, &data[0][0]}; - data[0][0] = 0; - data[0][1] = -v.z; - data[0][2] = v.y; - data[1][0] = v.z; - data[1][1] = 0; - data[1][2] = -v.x; - data[2][0] = -v.y; - data[2][1] = v.x; - data[2][2] = 0; - } - - /** - * access by index - */ - inline T &operator ()(unsigned int row, unsigned int col) { - return data[row][col]; - } - - /** - * access to elements by index - */ - inline const T &operator ()(unsigned int row, unsigned int col) const { - return data[row][col]; - } - - /** - * set to value - */ - const Matrix3 &operator =(const Matrix3 &m) { - memcpy(data, m.data, sizeof(data)); - return *this; - } - - /** - * test for equality - */ - bool operator ==(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return false; - return true; - } - - /** - * test for inequality - */ - bool operator !=(const Matrix3 &m) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (data[i][j] != m(i, j)) - return true; - return false; - } - - /** - * negation - */ - Matrix3 operator -(void) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = -data[i][j]; - return res; - } - - /** - * addition - */ - Matrix3 operator +(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] + m(i, j); - return res; - } - - Matrix3 &operator +=(const Matrix3 &m) { - return *this = *this + m; - } - - /** - * subtraction - */ - Matrix3 operator -(const Matrix3 &m) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] - m(i, j); - return res; - } - - Matrix3 &operator -=(const Matrix3 &m) { - return *this = *this - m; - } - - /** - * uniform scaling - */ - Matrix3 operator *(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] * num; - return res; - } - - Matrix3 &operator *=(const T num) { - return *this = *this * num; - } - - Matrix3 operator /(const T num) const { - Matrix3 res; - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - res[i][j] = data[i][j] / num; - return res; - } - - Matrix3 &operator /=(const T num) { - return *this = *this / num; - } - - /** - * multiplication by a vector - */ - Vector3 operator *(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z, - data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z, - data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z); - } - - /** - * multiplication of transpose by a vector - */ - Vector3 mul_transpose(const Vector3 &v) const { - return Vector3( - data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z, - data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z, - data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z); - } - - /** - * multiplication by another matrix - */ - Matrix3 operator *(const Matrix3 &m) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) - Matrix3 res; - arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0), - data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1), - data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2), - data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0), - data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1), - data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2), - data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0), - data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1), - data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2)); -#endif - } - - Matrix3 &operator *=(const Matrix3 &m) { - return *this = *this * m; - } - - /** - * transpose the matrix - */ - Matrix3 transposed(void) const { -#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float - Matrix3 res; - arm_mat_trans_f32(&arm_mat, &res.arm_mat); - return res; -#else - return Matrix3(data[0][0], data[1][0], data[2][0], - data[0][1], data[1][1], data[2][1], - data[0][2], data[1][2], data[2][2]); -#endif - } - - /** - * inverse the matrix - */ - Matrix3 inversed(void) const { - Matrix3 res; - arm_mat_inverse_f32(&arm_mat, &res.arm_mat); - return res; - } - - /** - * zero the matrix - */ - void zero(void) { - memset(data, 0, sizeof(data)); - } - - /** - * setup the identity matrix - */ - void identity(void) { - memset(data, 0, sizeof(data)); - data[0][0] = 1; - data[1][1] = 1; - data[2][2] = 1; - } - - /** - * check if any elements are NAN - */ - bool is_nan(void) { - for (int i = 0; i < 3; i++) - for (int j = 0; j < 3; j++) - if (isnan(data[i][j])) - return true; - return false; - } - - /** - * create a rotation matrix from given euler angles - * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf - */ - void from_euler(T roll, T pitch, T yaw) { - T cp = (T)cosf(pitch); - T sp = (T)sinf(pitch); - T sr = (T)sinf(roll); - T cr = (T)cosf(roll); - T sy = (T)sinf(yaw); - T cy = (T)cosf(yaw); - - data[0][0] = cp * cy; - data[0][1] = (sr * sp * cy) - (cr * sy); - data[0][2] = (cr * sp * cy) + (sr * sy); - data[1][0] = cp * sy; - data[1][1] = (sr * sp * sy) + (cr * cy); - data[1][2] = (cr * sp * sy) - (sr * cy); - data[2][0] = -sp; - data[2][1] = sr * cp; - data[2][2] = cr * cp; - } - - // create eulers from a rotation matrix - //void to_euler(float *roll, float *pitch, float *yaw); - - // apply an additional rotation from a body frame gyro vector - // to a rotation matrix. - //void rotate(const Vector3 &g); -}; - -typedef Matrix3 Matrix3f; -} - -#endif // MATRIX3_HPP diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index 64b980ad8..1b0cb3c41 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -44,15 +44,17 @@ #include #include "../CMSIS/Include/arm_math.h" +#include "Vector.hpp" +#include "Matrix.hpp" namespace math { -class Quaternion { -public: - float real; - Vector3 imag; +template +class Matrix; +class Quaternion : public Vector<4> { +public: /** * trivial ctor */ @@ -62,7 +64,74 @@ public: /** * setting ctor */ - Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) { + Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) { + } + + Quaternion(const Vector<4> &v) : Vector(v) { + } + + Quaternion(const float *v) : Vector(v) { + } + + /** + * access to elements by index + */ + /* + inline float &operator ()(unsigned int i) { + return *(&a + i); + } + */ + + /** + * access to elements by index + */ + /* + inline const float &operator ()(unsigned int i) const { + return *(&a + i); + } + */ + + /** + * addition + */ + /* + const Quaternion operator +(const Quaternion &q) const { + return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d); + } + */ + + /** + * subtraction + */ + /* + const Quaternion operator -(const Quaternion &q) const { + return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d); + } + */ + + Quaternion derivative(const Vector<3> &w) { + float dataQ[] = { + data[0], -data[1], -data[2], -data[3], + data[1], data[0], -data[3], data[2], + data[2], data[3], data[0], -data[1], + data[3], -data[2], data[1], data[0] + }; + Matrix<4,4> Q(dataQ); + Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]); + return Q * v * 0.5f; + } + + void from_euler(float roll, float pitch, float yaw) { + double cosPhi_2 = cos(double(roll) / 2.0); + double sinPhi_2 = sin(double(roll) / 2.0); + double cosTheta_2 = cos(double(pitch) / 2.0); + double sinTheta_2 = sin(double(pitch) / 2.0); + double cosPsi_2 = cos(double(yaw) / 2.0); + double sinPsi_2 = sin(double(yaw) / 2.0); + data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2; + data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2; + data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2; + data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2; } }; } diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp index cd00058b5..2df87c74b 100644 --- a/src/lib/mathlib/math/Vector.hpp +++ b/src/lib/mathlib/math/Vector.hpp @@ -42,38 +42,50 @@ #ifndef VECTOR_HPP #define VECTOR_HPP +#include #include #include "../CMSIS/Include/arm_math.h" namespace math { +template +class Vector; + template class VectorBase { public: + /** + * vector data + */ float data[N]; + + /** + * struct for using arm_math functions, represents column vector + */ arm_matrix_instance_f32 arm_col; /** * trivial ctor */ - VectorBase() { + VectorBase() { arm_col = {N, 1, &data[0]}; } /** - * setting ctor + * copy ctor */ -// VectorBase(const float *d) { - // memcpy(data, d, sizeof(data)); - //arm_col = {N, 1, &data[0]}; - //} + VectorBase(const VectorBase &v) { + arm_col = {N, 1, &data[0]}; + memcpy(data, v.data, sizeof(data)); + } /** * setting ctor */ - VectorBase(const float d[]) : data(d) { + VectorBase(const float *d) { arm_col = {N, 1, &data[0]}; + memcpy(data, d, sizeof(data)); } /** @@ -90,10 +102,18 @@ public: return data[i]; } + unsigned int getRows() { + return N; + } + + unsigned int getCols() { + return 1; + } + /** * test for equality */ - bool operator ==(const VectorBase &v) { + bool operator ==(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return false; @@ -103,7 +123,7 @@ public: /** * test for inequality */ - bool operator !=(const VectorBase &v) { + bool operator !=(const Vector &v) { for (unsigned int i = 0; i < N; i++) if (data[i] != v(i)) return true; @@ -113,98 +133,98 @@ public: /** * set to value */ - const VectorBase &operator =(const VectorBase &v) { + const Vector &operator =(const Vector &v) { memcpy(data, v.data, sizeof(data)); - return *this; + return *reinterpret_cast*>(this); } /** * negation */ - const VectorBase operator -(void) const { - VectorBase res; + const Vector operator -(void) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = -data[i]; + res.data[i] = -data[i]; return res; } /** * addition */ - const VectorBase operator +(const VectorBase &v) const { - VectorBase res; + const Vector operator +(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] + v(i); + res.data[i] = data[i] + v(i); return res; } /** * subtraction */ - const VectorBase operator -(const VectorBase &v) const { - VectorBase res; + const Vector operator -(const Vector &v) const { + Vector res; for (unsigned int i = 0; i < N; i++) - res[i] = data[i] - v(i); + res.data[i] = data[i] - v(i); return res; } /** * uniform scaling */ - const VectorBase operator *(const float num) const { - VectorBase temp(*this); + const Vector operator *(const float num) const { + Vector temp(*this); return temp *= num; } /** * uniform scaling */ - const VectorBase operator /(const float num) const { - VectorBase temp(*this); + const Vector operator /(const float num) const { + Vector temp(*reinterpret_cast*>(this)); return temp /= num; } /** * addition */ - const VectorBase &operator +=(const VectorBase &v) { + const Vector &operator +=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] += v(i); - return *this; + return *reinterpret_cast*>(this); } /** * subtraction */ - const VectorBase &operator -=(const VectorBase &v) { + const Vector &operator -=(const Vector &v) { for (unsigned int i = 0; i < N; i++) data[i] -= v(i); - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator *=(const float num) { + const Vector &operator *=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] *= num; - return *this; + return *reinterpret_cast*>(this); } /** * uniform scaling */ - const VectorBase &operator /=(const float num) { + const Vector &operator /=(const float num) { for (unsigned int i = 0; i < N; i++) data[i] /= num; - return *this; + return *reinterpret_cast*>(this); } /** * dot product */ - float operator *(const VectorBase &v) const { - float res; + float operator *(const Vector &v) const { + float res = 0.0f; for (unsigned int i = 0; i < N; i++) res += data[i] * v(i); return res; @@ -221,7 +241,7 @@ public: * gets the length of this vector */ float length() const { - return sqrtf(*this * *this); + return sqrtf(*this * *reinterpret_cast*>(this)); } /** @@ -234,46 +254,174 @@ public: /** * returns the normalized version of this vector */ - VectorBase normalized() const { + Vector normalized() const { return *this / length(); } + + void print(void) { + printf("[ "); + for (unsigned int i = 0; i < N; i++) + printf("%.3f\t", data[i]); + printf("]\n"); + } }; template class Vector : public VectorBase { public: + using VectorBase::operator *; + + Vector() : VectorBase() { + } + + Vector(const float d[]) : VectorBase(d) { + } + + Vector(const Vector &v) : VectorBase(v) { + } + + Vector(const VectorBase &v) : VectorBase(v) { + } + /** * set to value */ const Vector &operator =(const Vector &v) { + this->arm_col = {N, 1, &this->data[0]}; memcpy(this->data, v.data, sizeof(this->data)); return *this; } }; +template <> +class Vector<2> : public VectorBase<2> { +public: + Vector() : VectorBase<2>() { + } + + Vector(const float x, const float y) : VectorBase() { + data[0] = x; + data[1] = y; + } + + Vector(const Vector<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + Vector(const VectorBase<2> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + } + + /** + * set to value + */ + const Vector<2> &operator =(const Vector<2> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + return *this; + } + + float cross(const Vector<2> &b) const { + return data[0]*b.data[1] - data[1]*b.data[0]; + } + + float operator %(const Vector<2> &v) const { + return cross(v); + } + +}; + template <> class Vector<3> : public VectorBase<3> { public: - Vector<3>() { + Vector() { arm_col = {3, 1, &this->data[0]}; } - Vector<3>(const float x, const float y, const float z) { + Vector(const float x, const float y, const float z) { + arm_col = {3, 1, &this->data[0]}; data[0] = x; data[1] = y; data[2] = z; + } + + Vector(const Vector<3> &v) : VectorBase<3>() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + } + + /** + * setting ctor + */ + Vector(const float d[]) { arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; + } + + /** + * set to value + */ + const Vector<3> &operator =(const Vector<3> &v) { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + return *this; + } +}; + +template <> +class Vector<4> : public VectorBase<4> { +public: + Vector() : VectorBase() {} + + Vector(const float x, const float y, const float z, const float t) : VectorBase() { + data[0] = x; + data[1] = y; + data[2] = z; + data[3] = t; } + Vector(const Vector<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + Vector(const VectorBase<4> &v) : VectorBase() { + data[0] = v.data[0]; + data[1] = v.data[1]; + data[2] = v.data[2]; + data[3] = v.data[3]; + } + + /** + * setting ctor + */ + /* + Vector(const float d[]) { + arm_col = {3, 1, &this->data[0]}; + data[0] = d[0]; + data[1] = d[1]; + data[2] = d[2]; + } +*/ /** * set to value */ + /* const Vector<3> &operator =(const Vector<3> &v) { data[0] = v.data[0]; data[1] = v.data[1]; data[2] = v.data[2]; return *this; } + */ }; } diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp deleted file mode 100644 index 501740614..000000000 --- a/src/lib/mathlib/math/Vector2.hpp +++ /dev/null @@ -1,269 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Vector2.hpp - * - * 2D Vector - */ - -#ifndef VECTOR2_HPP -#define VECTOR2_HPP - -#include - -namespace math -{ - -template -class Vector2 { -public: - T x, y; - - /** - * trivial ctor - */ - Vector2() { - } - - /** - * setting ctor - */ - Vector2(const T x0, const T y0): x(x0), y(y0) { - } - - /** - * setter - */ - void set(const T x0, const T y0) { - x = x0; - y = y0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector2 &v) { - return (x == v.x && y == v.y); - } - - /** - * test for inequality - */ - bool operator !=(const Vector2 &v) { - return (x != v.x || y != v.y); - } - - /** - * set to value - */ - const Vector2 &operator =(const Vector2 &v) { - x = v.x; - y = v.y; - return *this; - } - - /** - * negation - */ - const Vector2 operator -(void) const { - return Vector2(-x, -y); - } - - /** - * addition - */ - const Vector2 operator +(const Vector2 &v) const { - return Vector2(x + v.x, y + v.y); - } - - /** - * subtraction - */ - const Vector2 operator -(const Vector2 &v) const { - return Vector2(x - v.x, y - v.y); - } - - /** - * uniform scaling - */ - const Vector2 operator *(const T num) const { - Vector2 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector2 operator /(const T num) const { - Vector2 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector2 &operator +=(const Vector2 &v) { - x += v.x; - y += v.y; - return *this; - } - - /** - * subtraction - */ - const Vector2 &operator -=(const Vector2 &v) { - x -= v.x; - y -= v.y; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator *=(const T num) { - x *= num; - y *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector2 &operator /=(const T num) { - x /= num; - y /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector2 &v) const { - return x * v.x + y * v.y; - } - - /** - * cross product - */ - const float operator %(const Vector2 &v) const { - return x * v.y - y * v.x; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector2 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector2 &n) - { - Vector2 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector2 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector2 projected(const Vector2 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector2 &v1, const Vector2 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector2 &v1, const Vector2 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector2 Vector2f; -} - -#endif // VECTOR2_HPP diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp deleted file mode 100644 index a0c3b9290..000000000 --- a/src/lib/mathlib/math/Vector3.hpp +++ /dev/null @@ -1,287 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Will Perone - * Anton Babushkin - * - * 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 Vector3.hpp - * - * 3D Vector - */ - -#ifndef VECTOR3_HPP -#define VECTOR3_HPP - -#include -#include "../CMSIS/Include/arm_math.h" - -namespace math -{ - -template -class Vector3 { -public: - T x, y, z; - arm_matrix_instance_f32 arm_col; - - /** - * trivial ctor - */ - Vector3() { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T x0, const T y0, const T z0): x(x0), y(y0), z(z0) { - arm_col = {3, 1, &x}; - } - - /** - * setting ctor - */ - Vector3(const T data[3]): x(data[0]), y(data[1]), z(data[2]) { - arm_col = {3, 1, &x}; - } - - /** - * setter - */ - void set(const T x0, const T y0, const T z0) { - x = x0; - y = y0; - z = z0; - } - - /** - * access to elements by index - */ - T operator ()(unsigned int i) { - return *(&x + i); - } - - /** - * access to elements by index - */ - const T operator ()(unsigned int i) const { - return *(&x + i); - } - - /** - * test for equality - */ - bool operator ==(const Vector3 &v) { - return (x == v.x && y == v.y && z == v.z); - } - - /** - * test for inequality - */ - bool operator !=(const Vector3 &v) { - return (x != v.x || y != v.y || z != v.z); - } - - /** - * set to value - */ - const Vector3 &operator =(const Vector3 &v) { - x = v.x; - y = v.y; - z = v.z; - return *this; - } - - /** - * negation - */ - const Vector3 operator -(void) const { - return Vector3(-x, -y, -z); - } - - /** - * addition - */ - const Vector3 operator +(const Vector3 &v) const { - return Vector3(x + v.x, y + v.y, z + v.z); - } - - /** - * subtraction - */ - const Vector3 operator -(const Vector3 &v) const { - return Vector3(x - v.x, y - v.y, z - v.z); - } - - /** - * uniform scaling - */ - const Vector3 operator *(const T num) const { - Vector3 temp(*this); - return temp *= num; - } - - /** - * uniform scaling - */ - const Vector3 operator /(const T num) const { - Vector3 temp(*this); - return temp /= num; - } - - /** - * addition - */ - const Vector3 &operator +=(const Vector3 &v) { - x += v.x; - y += v.y; - z += v.z; - return *this; - } - - /** - * subtraction - */ - const Vector3 &operator -=(const Vector3 &v) { - x -= v.x; - y -= v.y; - z -= v.z; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator *=(const T num) { - x *= num; - y *= num; - z *= num; - return *this; - } - - /** - * uniform scaling - */ - const Vector3 &operator /=(const T num) { - x /= num; - y /= num; - z /= num; - return *this; - } - - /** - * dot product - */ - T operator *(const Vector3 &v) const { - return x * v.x + y * v.y + z * v.z; - } - - /** - * cross product - */ - const Vector3 operator %(const Vector3 &v) const { - Vector3 temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x); - return temp; - } - - /** - * gets the length of this vector squared - */ - float length_squared() const { - return (*this * *this); - } - - /** - * gets the length of this vector - */ - float length() const { - return (T)sqrt(*this * *this); - } - - /** - * normalizes this vector - */ - void normalize() { - *this /= length(); - } - - /** - * returns the normalized version of this vector - */ - Vector3 normalized() const { - return *this / length(); - } - - /** - * reflects this vector about n - */ - void reflect(const Vector3 &n) - { - Vector3 orig(*this); - project(n); - *this = *this * 2 - orig; - } - - /** - * projects this vector onto v - */ - void project(const Vector3 &v) { - *this = v * (*this * v) / (v * v); - } - - /** - * returns this vector projected onto v - */ - Vector3 projected(const Vector3 &v) { - return v * (*this * v) / (v * v); - } - - /** - * computes the angle between 2 arbitrary vectors - */ - static inline float angle(const Vector3 &v1, const Vector3 &v2) { - return acosf((v1 * v2) / (v1.length() * v2.length())); - } - - /** - * computes the angle between 2 arbitrary normalized vectors - */ - static inline float angle_normalized(const Vector3 &v1, const Vector3 &v2) { - return acosf(v1 * v2); - } -}; - -typedef Vector3 Vector3f; -} - -#endif // VECTOR3_HPP diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h index d8e95b46b..9e03855c5 100644 --- a/src/lib/mathlib/mathlib.h +++ b/src/lib/mathlib/mathlib.h @@ -42,10 +42,7 @@ #pragma once #include "math/Vector.hpp" -#include "math/Vector2.hpp" -#include "math/Vector3.hpp" #include "math/Matrix.hpp" -#include "math/Matrix3.hpp" #include "math/Quaternion.hpp" #include "math/Limits.hpp" diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk index 50951c617..191e2da73 100644 --- a/src/lib/mathlib/module.mk +++ b/src/lib/mathlib/module.mk @@ -35,7 +35,6 @@ # Math library # SRCS = math/test/test.cpp \ - math/Matrix3.cpp \ math/Limits.cpp # diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index ecca04dd7..6533eb7cf 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -54,17 +54,17 @@ static const int8_t ret_error = -1; // error occurred KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : SuperBlock(parent, name), // ekf matrices - F(9, 9), - G(9, 6), - P(9, 9), - P0(9, 9), - V(6, 6), + F(), + G(), + P(), + P0(), + V(), // attitude measurement ekf matrices - HAtt(4, 9), - RAtt(4, 4), + HAtt(), + RAtt(), // position measurement ekf matrices - HPos(6, 9), - RPos(6, 6), + HPos(), + RPos(), // attitude representations C_nb(), q(), @@ -113,7 +113,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : using namespace math; // initial state covariance matrix - P0 = Matrix::identity(9) * 0.01f; + P0.identity(); + P0 *= 0.01f; P = P0; // initial state @@ -138,7 +139,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _sensors.magnetometer_ga[2]); // initialize dcm - C_nb = Dcm(q); + C_nb.from_quaternion(q); // HPos is constant HPos(0, 3) = 1.0f; @@ -404,28 +405,28 @@ int KalmanNav::predictState(float dt) // attitude prediction if (_attitudeInitialized) { - Vector3 w(_sensors.gyro_rad_s); + Vector<3> w(_sensors.gyro_rad_s); // attitude q = q + q.derivative(w) * dt; // renormalize quaternion if needed - if (fabsf(q.norm() - 1.0f) > 1e-4f) { - q = q.unit(); + if (fabsf(q.length() - 1.0f) > 1e-4f) { + q.normalize(); } // C_nb update - C_nb = Dcm(q); + C_nb.from_quaternion(q); // euler update - EulerAngles euler(C_nb); - phi = euler.getPhi(); - theta = euler.getTheta(); - psi = euler.getPsi(); + Vector<3> euler = C_nb.to_euler(); + phi = euler.data[0]; + theta = euler.data[1]; + psi = euler.data[2]; // specific acceleration in nav frame - Vector3 accelB(_sensors.accelerometer_m_s2); - Vector3 accelN = C_nb * accelB; + Vector<3> accelB(_sensors.accelerometer_m_s2); + Vector<3> accelN = C_nb * accelB; fN = accelN(0); fE = accelN(1); fD = accelN(2); @@ -549,10 +550,10 @@ int KalmanNav::predictStateCovariance(float dt) G(5, 4) = C_nb(2, 1); G(5, 5) = C_nb(2, 2); - // continuous predictioon equations - // for discrte time EKF + // continuous prediction equations + // for discrete time EKF // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt; + P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; return ret_ok; } @@ -577,13 +578,14 @@ int KalmanNav::correctAtt() // compensate roll and pitch, but not yaw // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose(); + math::Matrix<3,3> C_rp; + C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); // mag measurement - Vector3 magBody(_sensors.magnetometer_ga); + Vector<3> magBody(_sensors.magnetometer_ga); // transform to earth frame - Vector3 magNav = C_rp * magBody; + Vector<3> magNav = C_rp * magBody; // calculate error between estimate and measurement // apply declination correction for true heading as well. @@ -592,12 +594,12 @@ int KalmanNav::correctAtt() if (yMag < -M_PI_F) yMag += 2*M_PI_F; // accel measurement - Vector3 zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.norm(); - zAccel = zAccel.unit(); + Vector<3> zAccel(_sensors.accelerometer_m_s2); + float accelMag = zAccel.length(); + zAccel.normalize(); // ignore accel correction when accel mag not close to g - Matrix RAttAdjust = RAtt; + Matrix<4,4> RAttAdjust = RAtt; bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; @@ -611,14 +613,10 @@ int KalmanNav::correctAtt() } // accel predicted measurement - Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit(); + Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); // calculate residual - Vector y(4); - y(0) = yMag; - y(1) = zAccel(0) - zAccelHat(0); - y(2) = zAccel(1) - zAccelHat(1); - y(3) = zAccel(2) - zAccelHat(2); + Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); // HMag HAtt(0, 2) = 1; @@ -632,9 +630,9 @@ int KalmanNav::correctAtt() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance - Matrix K = P * HAtt.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance + Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correciton is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -669,7 +667,7 @@ int KalmanNav::correctAtt() P = P - K * HAtt * P; // fault detection - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); if (beta > _faultAtt.get()) { warnx("fault in attitude: beta = %8.4f", (double)beta); @@ -678,7 +676,7 @@ int KalmanNav::correctAtt() // update quaternions from euler // angle correction - q = Quaternion(EulerAngles(phi, theta, psi)); + q.from_euler(phi, theta, psi); return ret_ok; } @@ -688,7 +686,7 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(6); + Vector<6> y; y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; @@ -698,9 +696,9 @@ int KalmanNav::correctPos() // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance - Matrix K = P * HPos.transpose() * S.inverse(); - Vector xCorrect = K * y; + Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance + Matrix<9,6> K = P * HPos.transposed() * S.inversed(); + Vector<9> xCorrect = K * y; // check correction is sane for (size_t i = 0; i < xCorrect.getRows(); i++) { @@ -735,7 +733,7 @@ int KalmanNav::correctPos() P = P - K * HPos * P; // fault detetcion - float beta = y.dot(S.inverse() * y); + float beta = y * (S.inversed() * y); static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index a69bde1a6..46ee4b6c8 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -125,17 +125,17 @@ public: virtual void updateParams(); protected: // kalman filter - math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix G; /**< noise shaping matrix for gyro/accel */ - math::Matrix P; /**< state covariance matrix */ - math::Matrix P0; /**< initial state covariance matrix */ - math::Matrix V; /**< gyro/ accel noise matrix */ - math::Matrix HAtt; /**< attitude measurement matrix */ - math::Matrix RAtt; /**< attitude measurement noise matrix */ - math::Matrix HPos; /**< position measurement jacobian matrix */ - math::Matrix RPos; /**< position measurement noise matrix */ + math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ + math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ + math::Matrix<9,9> P; /**< state covariance matrix */ + math::Matrix<9,9> P0; /**< initial state covariance matrix */ + math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ + math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ + math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ + math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ + math::Matrix<6,6> RPos; /**< position measurement noise matrix */ // attitude - math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */ + math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ math::Quaternion q; /**< quaternion from body to nav frame */ // subscriptions control::UOrbSubscription _sensors; /**< sensors sub. */ diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5eeca5a1a..36b75dd58 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd) 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); + math::Matrix<3,3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - 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_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_t * accel_T_mat * board_rotation; + math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Vector<3> accel_offs_vec(&accel_offs[0]); + math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; + math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3,3> 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); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7b6fad658..653d4b6b3 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -683,8 +683,9 @@ handle_message(mavlink_message_t *msg) /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); - math::Dcm C_nb(q); - math::EulerAngles euler(C_nb); + math::Matrix<3,3> C_nb; + C_nb.from_quaternion(q); + math::Vector<3> euler = C_nb.to_euler(); /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) @@ -699,9 +700,9 @@ handle_message(mavlink_message_t *msg) hil_attitude.q[3] = q(3); hil_attitude.q_valid = true; - hil_attitude.roll = euler.getPhi(); - hil_attitude.pitch = euler.getTheta(); - hil_attitude.yaw = euler.getPsi(); + hil_attitude.roll = euler(0); + hil_attitude.pitch = euler(1); + hil_attitude.yaw = euler(2); hil_attitude.rollspeed = hil_state.rollspeed; hil_attitude.pitchspeed = hil_state.pitchspeed; hil_attitude.yawspeed = hil_state.yawspeed; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f6c44444a..04307d96b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -422,11 +422,11 @@ Navigator::task_main() mission_poll(); - math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); // Current waypoint - math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); + math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f); // Global position - math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); + math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* AUTONOMOUS FLIGHT */ @@ -436,19 +436,19 @@ Navigator::task_main() if (_mission_valid) { // Next waypoint - math::Vector2f prev_wp; + math::Vector<2> prev_wp; if (_global_triplet.previous_valid) { - prev_wp.setX(_global_triplet.previous.lat / 1e7f); - prev_wp.setY(_global_triplet.previous.lon / 1e7f); + prev_wp(0) = _global_triplet.previous.lat / 1e7f; + prev_wp(1) = _global_triplet.previous.lon / 1e7f; } else { /* * No valid next waypoint, go for heading hold. * This is automatically handled by the L1 library. */ - prev_wp.setX(_global_triplet.current.lat / 1e7f); - prev_wp.setY(_global_triplet.current.lon / 1e7f); + prev_wp(0) = _global_triplet.current.lat / 1e7f; + prev_wp(1) = _global_triplet.current.lon / 1e7f; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6d38b98ec..5baab4a5d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -211,8 +211,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ struct { @@ -457,8 +457,8 @@ Sensors::Sensors() : /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), - _board_rotation(3, 3), - _external_mag_rotation(3, 3), + _board_rotation(), + _external_mag_rotation(), _mag_is_external(false) { @@ -904,7 +904,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); - math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z}; + math::Vector<3> vect = {accel_report.x, accel_report.y, accel_report.z}; vect = _board_rotation * vect; raw.accelerometer_m_s2[0] = vect(0); @@ -930,7 +930,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z}; + math::Vector<3> vect = {gyro_report.x, gyro_report.y, gyro_report.z}; vect = _board_rotation * vect; raw.gyro_rad_s[0] = vect(0); @@ -956,7 +956,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; + math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); if (_mag_is_external) vect = _external_mag_rotation * vect; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 305ebbefa..d2e1a93e3 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -75,15 +75,6 @@ int test_mathlib(int argc, char *argv[]) Matrix<3,3> mres3; Matrix<4,4> mres4; - Matrix3f m3old; - m3old.identity(); - Vector3f v3old; - v3old.x = 1.0f; - v3old.y = 2.0f; - v3old.z = 3.0f; - Vector3f vres3old; - Matrix3f mres3old; - unsigned int n = 60000; hrt_abstime t0, t1; @@ -95,13 +86,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Vector3: %s %.6fus", formatResult(vres3 == v3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - vres3old = m3old * v3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Vector3 OLD: %s %.6fus", formatResult(vres3old == v3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres3 = m3 * m3; @@ -109,13 +93,6 @@ int test_mathlib(int argc, char *argv[]) t1 = hrt_absolute_time(); warnx("Matrix3 * Matrix3: %s %.6fus", formatResult(mres3 == m3), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); - for (unsigned int j = 0; j < n; j++) { - mres3old = m3old * m3old; - } - t1 = hrt_absolute_time(); - warnx("Matrix3 * Matrix3 OLD: %s %.6fus", formatResult(mres3old == m3old), (double)(t1 - t0) / n); - t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { mres4 = m4 * m4; -- cgit v1.2.3 From 32c7aea2a6a0c355d2cae362884e40e4f3573311 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 11:07:45 +0100 Subject: Home position: use double for lat/lon and float for altitude, set home position to global position instead of GPS position once we have a fix --- src/drivers/hott/messages.cpp | 4 ++-- src/modules/commander/commander.cpp | 20 +++++++------------- src/modules/mavlink/orb_listener.c | 2 +- src/modules/navigator/navigator_main.cpp | 21 ++++++++++----------- src/modules/uORB/topics/home_position.h | 20 +++++++++----------- 5 files changed, 29 insertions(+), 38 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index bb8d45bea..90a744015 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -295,8 +295,8 @@ build_gps_response(uint8_t *buffer, size_t *size) memset(&home, 0, sizeof(home)); orb_copy(ORB_ID(home_position), _home_sub, &home); - _home_lat = ((double)(home.lat))*1e-7d; - _home_lon = ((double)(home.lon))*1e-7d; + _home_lat = home.lat; + _home_lon = home.lon; _home_position_set = true; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3fc1d2c19..442f16a58 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1048,23 +1048,17 @@ int commander_thread_main(int argc, char *argv[]) if (!home_position_set && gps_position.fix_type >= 3 && (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) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed + && global_position.valid) { /* 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; - 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; + home.lat = (double)global_position.lat / 1e7d; + home.lon = (double)global_position.lon / 1e7d; + home.altitude = (float)global_position.alt / 1e3f; - 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); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index de902f3da..96888f06a 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -657,7 +657,7 @@ l_home(const struct listener *l) orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c7ac885b4..c6fe93e9e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -953,8 +953,7 @@ Navigator::start_loiter() get_loiter_item(&_mission_item_triplet.current); - /* XXX get rid of ugly conversion for home position altitude */ - float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f; + float global_min_alt = _parameters.min_altitude + _home_pos.altitude; /* Use current altitude if above min altitude set by parameter */ if (_global_pos.alt < global_min_alt) { @@ -1080,9 +1079,9 @@ Navigator::start_rtl() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; _mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; @@ -1104,9 +1103,9 @@ Navigator::start_rtl_loiter() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7; - _mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7; - _mission_item_triplet.current.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; get_loiter_item(&_mission_item_triplet.current); @@ -1319,9 +1318,9 @@ Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) { if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = (double)_home_pos.lat / 1e7; - new_mission_item->lon = (double)_home_pos.lon / 1e7; - new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number } diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 7e1b82a0f..1cf37e29c 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -2,6 +2,7 @@ * * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier + * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,9 +35,10 @@ /** * @file home_position.h - * Definition of the GPS home position uORB topic. + * Definition of the home position uORB topic. * * @author Lorenz Meier + * @author Julian Oes */ #ifndef TOPIC_HOME_POSITION_H_ @@ -55,16 +57,12 @@ */ struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp from the gps module */ - - int32_t lat; /**< Latitude in 1E7 degrees */ - int32_t lon; /**< Longitude in 1E7 degrees */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float p_variance_m; /**< position accuracy estimate m */ + uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ + + bool altitude_is_relative; + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float altitude; /**< Altitude in meters */ }; /** -- cgit v1.2.3 From 72d9c80ce954d2289282f5df01aef7e5e8914acc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Dec 2013 14:00:27 +0100 Subject: Home position: corrected wrong conversion --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 442f16a58..28118b990 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1055,7 +1055,7 @@ int commander_thread_main(int argc, char *argv[]) home.lat = (double)global_position.lat / 1e7d; home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt / 1e3f; + home.altitude = (float)global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); -- cgit v1.2.3 From 7e29028429fce33b88dd1eeb6d3ac89aa5cb5891 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Dec 2013 12:16:49 +0400 Subject: Moving nav state from commander to navigator, WIP --- src/modules/commander/commander.cpp | 278 +++----------------- src/modules/commander/state_machine_helper.cpp | 189 +------------- src/modules/commander/state_machine_helper.h | 9 +- src/modules/mavlink/mavlink.c | 3 + src/modules/mavlink_onboard/mavlink.c | 3 +- .../multirotor_att_control_main.c | 2 +- .../multirotor_pos_control.c | 134 +--------- src/modules/navigator/navigator_main.cpp | 285 ++++++++++++--------- src/modules/sdlog2/sdlog2.c | 3 +- src/modules/uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 18 +- 11 files changed, 250 insertions(+), 693 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 28118b990..199bfb0da 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -69,7 +69,6 @@ #include #include #include -#include #include #include #include @@ -194,7 +193,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -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, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -215,8 +214,6 @@ void print_reject_arm(const char *msg); void print_status(); -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. */ @@ -346,13 +343,12 @@ 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) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ - uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; + enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* only handle high-priority commands here */ @@ -365,12 +361,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = 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, mavlink_fd); /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -386,12 +382,12 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { + if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -401,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, control_mode, 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"); @@ -462,29 +458,6 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_NAV_TAKEOFF: { - 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_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - } else { - /* reject TAKEOFF not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - break; - } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -494,7 +467,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, control_mode, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -513,7 +486,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON, control_mode); + transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -524,29 +497,22 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: + case VEHICLE_CMD_PREFLIGHT_STORAGE: + /* ignore commands that handled in low prio loop */ + break; + default: + /* warn about unsupported commands */ + answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* supported command handling stop */ - 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(); - - if (result == VEHICLE_CMD_RESULT_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command); - - } else if (result == VEHICLE_CMD_RESULT_FAILED) { - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command); - - } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command); - - } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); } /* send any requested ACKs */ @@ -564,9 +530,6 @@ 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 */ @@ -609,11 +572,9 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* Initialize all flags to false */ - memset(&control_mode, 0, sizeof(control_mode)); - status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_DIRECT; + status.set_nav_state = NAV_STATE_INIT; + status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -625,9 +586,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status.offboard_control_signal_lost = true; - /* allow manual override initially */ - control_mode.flag_external_manual_override_ok = true; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; status.condition_battery_voltage_valid = false; @@ -635,9 +593,6 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized 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), &status); /* publish current state machine */ @@ -649,8 +604,6 @@ int commander_thread_main(int argc, char *argv[]) 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; @@ -803,11 +756,9 @@ int commander_thread_main(int argc, char *argv[]) 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; } @@ -859,7 +810,7 @@ int commander_thread_main(int argc, char *argv[]) // 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); + // (void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); // } } @@ -990,10 +941,10 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -1012,7 +963,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, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1102,15 +1053,13 @@ int commander_thread_main(int argc, char *argv[]) * 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 || - (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.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + 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, &control_mode, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1132,7 +1081,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, &control_mode, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1185,12 +1134,12 @@ int commander_thread_main(int argc, char *argv[]) /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON, &control_mode); + transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); if (flighttermination_res == TRANSITION_CHANGED) { tune_positive(); } } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF, &control_mode); + flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } @@ -1202,32 +1151,18 @@ int commander_thread_main(int argc, char *argv[]) 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); - - 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, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + handle_command(&status, &safety, &cmd, &armed); } /* 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(); bool flighttermination_state_changed = check_flighttermination_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (navigation_state_changed || arming_state_changed) { - control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - } - - 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); + if (arming_state_changed || main_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); status_changed = true; } @@ -1235,8 +1170,6 @@ int commander_thread_main(int argc, char *argv[]) if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1549,133 +1482,6 @@ print_reject_arm(const char *msg) } } -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) -{ - transition_result_t res = TRANSITION_DENIED; - - 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 */ - // XXX: only respect the condition_landed when the local position is actually valid - if (status->is_rotary_wing && status->condition_local_altitude_valid && (local_pos->z > -takeoff_alt || status->condition_landed)) { - 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) { - /* possibly on ground, switch to TAKEOFF if needed */ - if (status->is_rotary_wing && status->condition_local_altitude_valid && (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 { - 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 { - /* 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); - } - - } 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 && status->condition_local_position_valid) { - /* in air: try to hold position if possible */ - 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, "#audio: FAILSAFE: POS HOLD"); - - } else { - if (status->condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)"); - - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD"); - } - } - } - - } else { - 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; - - 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; -} - void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result) { switch (result) { @@ -1784,7 +1590,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, &control_mode, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1824,7 +1630,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); break; } @@ -1876,7 +1682,7 @@ void *commander_low_prio_loop(void *arg) } default: - answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); + /* don't answer on unsupported commands, it will be done in main loop */ break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ca3ec94b8..731e0e3ff 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,7 +46,6 @@ #include #include #include -#include #include #include #include @@ -64,12 +63,10 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool navigation_state_changed = true; static bool flighttermination_state_changed = true; transition_result_t 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) { /* @@ -86,7 +83,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { /* enforce lockdown in HIL */ - if (control_mode->flag_system_hil_enabled) { + if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; } else { armed->lockdown = false; @@ -109,7 +106,7 @@ 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 - || control_mode->flag_system_hil_enabled) { + || status->hil_state == HIL_STATE_ON) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -126,7 +123,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 || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -289,169 +286,6 @@ check_main_state_changed() } } -transition_result_t -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 == status->navigation_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_navigation_state) { - 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_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - 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_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - 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_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - 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_climb_rate_enabled = true; - control_mode->flag_control_manual_enabled = true; - control_mode->flag_control_auto_enabled = false; - break; - - case NAVIGATION_STATE_AUTO_READY: - 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; - control_mode->flag_control_auto_enabled = true; - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - 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: - 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 = false; - break; - - case NAVIGATION_STATE_AUTO_MISSION: - 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_RTL: - 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_LAND: - - /* deny transitions from landed state */ - 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; - } - - break; - - default: - break; - } - - if (ret == TRANSITION_CHANGED) { - status->navigation_state = new_navigation_state; - control_mode->auto_state = status->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; - } -} - bool check_flighttermination_state_changed() { @@ -464,16 +298,10 @@ check_flighttermination_state_changed() } } -void -set_navigation_state_changed() -{ - navigation_state_changed = true; -} - /** * Transition from one hil state to another */ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -502,7 +330,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s || 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"); valid_transition = true; } @@ -521,9 +348,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - 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; @@ -539,7 +363,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** * Transition from one flightermination state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode) +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) { transition_result_t ret = TRANSITION_DENIED; @@ -566,7 +390,8 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * if (ret == TRANSITION_CHANGED) { flighttermination_state_changed = true; - control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + // TODO + //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e1ec9d4ad..e569fb4f3 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -48,7 +48,6 @@ #include #include #include -#include typedef enum { TRANSITION_DENIED = -1, @@ -58,7 +57,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - const struct vehicle_control_mode_s *control_mode, 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,9 +67,7 @@ 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 *status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); - -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state, struct vehicle_control_mode_s *control_mode); +transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); bool check_navigation_state_changed(); @@ -78,6 +75,6 @@ bool check_flighttermination_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); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 2ec00a9bc..eec6c567c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -221,6 +221,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } 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; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO use control_mode topic + /* 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) { @@ -234,6 +236,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } 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; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 0edb53a3e..ab9ce45f3 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -441,7 +441,8 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + // TODO fix navigation state, use control_mode topic + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, 0, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, 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 60a211817..111e9197f 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -316,7 +316,7 @@ mc_thread_main(int argc, char *argv[]) } } else { - if (!control_mode.flag_control_auto_enabled) { + if (!control_mode.flag_control_attitude_enabled) { /* no control, try to stay on place */ if (!control_mode.flag_control_velocity_enabled) { /* no velocity control, reset attitude setpoint */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 5af7e2a82..2ca650420 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -415,140 +415,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* force reprojection of global setpoint after manual mode */ reset_mission_sp = true; - - } else if (control_mode.flag_control_auto_enabled) { - /* AUTO mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - if (reset_takeoff_sp) { - reset_takeoff_sp = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z); - } - - reset_auto_sp_xy = false; - reset_auto_sp_z = true; - - } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) { - // TODO - reset_auto_sp_xy = true; - reset_auto_sp_z = 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) { - reset_mission_sp = 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", (double)lat_home, (double)lon_home); - } - - if (reset_mission_sp) { - reset_mission_sp = false; - /* update global setpoint projection */ - - if (global_pos_sp_valid) { - - /* project global setpoint to local setpoint */ - map_projection_project(triplet.current.lat, triplet.current.lon, &(local_pos_sp.x), &(local_pos_sp.y)); - - if (triplet.current.altitude_is_relative) { - local_pos_sp.z = -triplet.current.altitude; - - } else { - local_pos_sp.z = local_pos.ref_alt - triplet.current.lat; - } - /* update yaw setpoint only if value is valid */ - if (isfinite(triplet.current.yaw) && fabsf(triplet.current.yaw) < M_TWOPI) { - att_sp.yaw_body = triplet.current.yaw; - } - - mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", triplet.current.lat, triplet.current.lon, (double)local_pos_sp.x, (double)local_pos_sp.y); - - } else { - if (reset_auto_sp_xy) { - reset_auto_sp_xy = false; - /* 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.yaw = att.yaw; - } - - if (reset_auto_sp_z) { - reset_auto_sp_z = false; - local_pos_sp.z = local_pos.z; - } - - mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - } - - reset_auto_sp_xy = true; - reset_auto_sp_z = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_TAKEOFF) { - reset_takeoff_sp = true; - } - - if (control_mode.auto_state != NAVIGATION_STATE_AUTO_MISSION) { - reset_mission_sp = true; - } - - /* copy yaw setpoint to vehicle_local_position_setpoint topic */ - local_pos_sp.yaw = att_sp.yaw_body; - - /* reset setpoints after AUTO mode */ - reset_man_sp_xy = true; - reset_man_sp_z = true; - - } else { - /* no control (failsafe), 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", (double) - local_pos_sp.z); - } - - reset_man_sp_z = true; - - } else { - /* in air: hold altitude */ - if (reset_man_sp_z) { - reset_man_sp_z = false; - local_pos_sp.z = local_pos.z; - mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z); - } - - reset_auto_sp_z = false; - } - - if (control_mode.flag_control_position_enabled) { - if (reset_man_sp_xy) { - reset_man_sp_xy = false; - local_pos_sp.x = local_pos.x; - local_pos_sp.y = local_pos.y; - local_pos_sp.yaw = att.yaw; - att_sp.yaw_body = att.yaw; - mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y); - } - - reset_auto_sp_xy = false; - } } + /* AUTO not implemented */ /* publish local position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7be9229c9..37c2a0389 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -145,8 +146,10 @@ private: orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ + orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ @@ -168,6 +171,8 @@ private: MissionFeasibilityChecker missionFeasiblityChecker; + uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + struct { float min_altitude; float loiter_radius; @@ -192,21 +197,10 @@ private: MAX_EVENT }; - enum State { - STATE_INIT, - STATE_NONE, - STATE_LOITER, - STATE_MISSION, - STATE_MISSION_LOITER, - STATE_RTL, - STATE_RTL_LOITER, - MAX_STATE - }; - /** * State machine transition table */ - static StateTable::Tran const myTable[MAX_STATE][MAX_EVENT]; + static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; /** * Update our local parameter cache. @@ -300,6 +294,10 @@ private: */ void publish_mission_item_triplet(); + /** + * Publish vehicle_control_mode topic for controllers + */ + void publish_control_mode(); /** @@ -328,7 +326,7 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], MAX_STATE, MAX_EVENT), + StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), @@ -347,6 +345,7 @@ Navigator::Navigator() : _triplet_pub(-1), _fence_pub(-1), _mission_result_pub(-1), + _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -357,7 +356,8 @@ Navigator::Navigator() : _mission(), _waypoint_position_reached(false), _waypoint_yaw_reached(false), - _time_first_inside_orbit(0) + _time_first_inside_orbit(0), + _set_nav_state_timestamp(0) { memset(&_fence, 0, sizeof(_fence)); @@ -375,7 +375,7 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); /* Initialize state machine */ - myState = STATE_INIT; + myState = NAV_STATE_INIT; } Navigator::~Navigator() @@ -513,7 +513,6 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - /* copy all topics first time */ vehicle_status_update(); @@ -524,9 +523,6 @@ Navigator::task_main() offboard_mission_update(_vstatus.is_rotary_wing); onboard_mission_update(); - - /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -575,49 +571,41 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { - - switch (_vstatus.navigation_state) { - case NAVIGATION_STATE_DIRECT: - case NAVIGATION_STATE_STABILIZE: - case NAVIGATION_STATE_ALTHOLD: - case NAVIGATION_STATE_VECTOR: - - /* don't publish a mission item triplet */ - dispatch(EVENT_NONE_REQUESTED); - break; - - case NAVIGATION_STATE_AUTO_READY: - case NAVIGATION_STATE_AUTO_TAKEOFF: - case NAVIGATION_STATE_AUTO_MISSION: - - /* try mission if none is available, fallback to loiter instead */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_INIT: + case NAV_STATE_NONE: + /* nothing to do */ break; - case NAVIGATION_STATE_AUTO_LOITER: - + case NAV_STATE_LOITER: dispatch(EVENT_LOITER_REQUESTED); break; - - case NAVIGATION_STATE_AUTO_RTL: - - dispatch(EVENT_RTL_REQUESTED); + case NAV_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); break; - case NAVIGATION_STATE_AUTO_LAND: - - /* TODO add this */ - + case NAV_STATE_RTL: + dispatch(EVENT_RTL_REQUESTED); break; default: - warnx("ERROR: Navigation state not supported"); + warnx("ERROR: Requested navigation state not supported"); break; + } + + } else { + /* try mission, if none is available fallback to loiter instead */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; } } else { @@ -632,10 +620,8 @@ Navigator::task_main() } } - /* only update parameters if it changed */ if (fds[0].revents & POLLIN) { - parameters_update(); /* note that these new parameters won't be in effect until a mission triplet is published again */ } @@ -670,9 +656,7 @@ Navigator::task_main() if (mission_item_reached()) { if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.navigation_state == NAVIGATION_STATE_AUTO_READY || - _vstatus.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || - _vstatus.navigation_state == NAVIGATION_STATE_AUTO_MISSION)) { + (myState == NAV_STATE_MISSION)) { /* advance by one mission item */ _mission.move_to_next(); @@ -688,6 +672,9 @@ Navigator::task_main() } } } + + publish_control_mode(); + perf_end(_loop_perf); } @@ -740,25 +727,25 @@ Navigator::status() } switch (myState) { - case STATE_INIT: + case NAV_STATE_INIT: warnx("State: Init"); break; - case STATE_NONE: + case NAV_STATE_NONE: warnx("State: None"); break; - case STATE_LOITER: + case NAV_STATE_LOITER: warnx("State: Loiter"); break; - case STATE_MISSION: + case NAV_STATE_MISSION: warnx("State: Mission"); break; - case STATE_MISSION_LOITER: + case NAV_STATE_MISSION_LOITER: warnx("State: Loiter after Mission"); break; - case STATE_RTL: + case NAV_STATE_RTL: warnx("State: RTL"); break; - case STATE_RTL_LOITER: + case NAV_STATE_RTL_LOITER: warnx("State: Loiter after RTL"); break; default: @@ -857,76 +844,76 @@ Navigator::fence_point(int argc, char *argv[]) -StateTable::Tran const Navigator::myTable[MAX_STATE][MAX_EVENT] = { +StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_INIT */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_INIT}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_INIT}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_INIT}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, }, { /* STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_NONE}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_NONE}, + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, { /* STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_mission_loiter), NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, { /* STATE_MISSION_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_MISSION_LOITER}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, STATE_MISSION_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, + /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION_LOITER}, }, { /* STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_RTL}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_MISSION_FINISHED */ {ACTION(&Navigator::start_rtl_loiter), NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_RTL}, }, { /* STATE_RTL_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, STATE_RTL_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL_LOITER}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, }, }; @@ -949,7 +936,7 @@ Navigator::start_loiter() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? get_loiter_item(&_mission_item_triplet.current); @@ -1239,6 +1226,76 @@ Navigator::publish_mission_item_triplet() } } +void +Navigator::publish_control_mode() +{ + /* update vehicle_control_mode topic*/ + _control_mode.main_state = _vstatus.main_state; + _control_mode.nav_state = static_cast(myState); + _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; + _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; + _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; + + _control_mode.flag_control_offboard_enabled = false; + _control_mode.flag_control_flighttermination_enabled = false; + + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: + break; + } + + _control_mode.timestamp = hrt_absolute_time(); + + /* lazily publish the mission triplet only once available */ + if (_control_mode_pub > 0) { + /* publish the mission triplet */ + orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); + + } else { + /* advertise and publish */ + _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); + } +} bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9f634d9e4..78322aff3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -969,7 +969,8 @@ int sdlog2_thread_main(int argc, char *argv[]) // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; + // TODO use control_mode topic + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; log_msg.body.log_STAT.battery_current = buf_status.battery_current; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index e26fb97c8..5ce968f67 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -48,6 +48,7 @@ #include #include #include "../uORB.h" +#include "vehicle_status.h" /** * @addtogroup topics @{ @@ -59,10 +60,25 @@ * * Encodes the complete system state and is set by the commander app. */ + +typedef enum { + NAV_STATE_INIT = 0, + NAV_STATE_NONE, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_MISSION_LOITER, + NAV_STATE_RTL, + NAV_STATE_RTL_LOITER, + NAV_STATE_MAX +} nav_state_t; + struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + main_state_t main_state; + nav_state_t nav_state; + bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -79,9 +95,6 @@ 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 flag_control_flighttermination_enabled; /**< true if flighttermination 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 1c3763924..ae3a24a1b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -66,20 +66,6 @@ typedef enum { MAIN_STATE_AUTO, } main_state_t; -/* navigation state machine */ -typedef enum { - 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 - 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, ARMING_STATE_STANDBY, @@ -95,7 +81,6 @@ typedef enum { HIL_STATE_ON } hil_state_t; - typedef enum { FLIGHTTERMINATION_STATE_OFF = 0, FLIGHTTERMINATION_STATE_ON @@ -182,7 +167,8 @@ struct vehicle_status_s uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t navigation_state; /**< navigation state machine */ + unsigned int set_nav_state; /**< set navigation state machine to specified value */ + uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ -- cgit v1.2.3 From fa20fae6fb627d19d9f3951e75058eb85ab8a10a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 29 Dec 2013 22:38:22 +0400 Subject: Some mavlink fixes to use vehicle_control_mode, WIP --- src/modules/commander/commander.cpp | 39 ++++++++++++++++++++++++++++++++++--- src/modules/mavlink/mavlink.c | 20 ++++++++----------- src/modules/mavlink/orb_listener.c | 27 +++++++++++++++++++++++++ src/modules/mavlink/orb_topics.h | 4 ++++ 4 files changed, 75 insertions(+), 15 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 199bfb0da..98979df3e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -193,7 +193,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -345,10 +345,11 @@ void print_status() static orb_advert_t status_pub; -void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; + bool ret = false; /* only handle high-priority commands here */ @@ -375,6 +376,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) + ret = true; // TODO remove debug code //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); @@ -407,6 +410,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) + ret = true; /* set main state */ transition_result_t main_res = TRANSITION_DENIED; @@ -447,6 +452,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) + ret = true; if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -473,6 +480,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; } else { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); @@ -482,12 +490,36 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_OVERRIDE_GOTO: { + // TODO listen vehicle_command topic directly from navigator (?) + unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD + status->set_nav_state = NAV_STATE_LOITER; + status->set_nav_state_timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; + + } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE + status->set_nav_state = NAV_STATE_MISSION; + status->set_nav_state_timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; + + } else { + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + } + } + break; + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); result = VEHICLE_CMD_RESULT_ACCEPTED; + ret = true; } else { /* reject parachute depoyment not armed */ @@ -1151,7 +1183,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &safety, &cmd, &armed); + if (handle_command(&status, &safety, &cmd, &armed)) + status_changed = true; } /* check which state machines for changes, clear "changed" flag */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4c38cf35a..bc191f30d 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,22 +220,18 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } 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; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - // TODO use control_mode topic - /* - if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + // TODO review + if (control_mode.nav_state == NAV_STATE_NONE) { 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) { + } else if (control_mode.nav_state == NAV_STATE_LOITER) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { + } else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) { + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + } else if (control_mode.nav_state == NAV_STATE_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; diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 28478a803..e168dded3 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -69,6 +69,7 @@ struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; +struct vehicle_control_mode_s control_mode; struct rc_channels_s rc; struct rc_input_values rc_raw; struct actuator_armed_s armed; @@ -125,6 +126,7 @@ static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); static void l_nav_cap(const struct listener *l); +static void l_control_mode(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -151,6 +153,7 @@ static const struct listener listeners[] = { {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, + {l_control_mode, &mavlink_subs.control_mode_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -678,6 +681,26 @@ l_nav_cap(const struct listener *l) } +void +l_control_mode(const struct listener *l) +{ + orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode); + + /* translate the current syste state to mavlink state and mode */ + uint8_t mavlink_state = 0; + 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_base_mode, + mavlink_custom_mode, + mavlink_state); +} + static void * uorb_receive_thread(void *arg) { @@ -753,6 +776,10 @@ uorb_receive_start(void) status_sub = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */ + /* --- CONTROL MODE --- */ + mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */ + /* --- RC CHANNELS VALUE --- */ rc_sub = orb_subscribe(ORB_ID(rc_channels)); orb_set_interval(rc_sub, 100); /* 10Hz updates */ diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 9000728cb..4d428406a 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -95,6 +95,7 @@ struct mavlink_subscriptions { int home_sub; int airspeed_sub; int navigation_capabilities_sub; + int control_mode_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -111,6 +112,9 @@ extern struct navigation_capabilities_s nav_cap; /** Vehicle status */ extern struct vehicle_status_s v_status; +/** Vehicle control mode */ +extern struct vehicle_control_mode_s control_mode; + /** RC channels */ extern struct rc_channels_s rc; -- cgit v1.2.3 From 33385a783cec5045a910e4890fa8c8f4b2fc7641 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Dec 2013 14:04:24 +0400 Subject: Added NONE = not mapped state for mission and return switches --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/sensors/sensors.cpp | 24 ++++++++++++------------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 18 insertions(+), 16 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 98979df3e..03d3c02d1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1392,7 +1392,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->mode_switch = MODE_SWITCH_ASSISTED; } - /* land switch */ + /* return switch */ if (!isfinite(sp_man->return_switch)) { current_status->return_switch = RETURN_SWITCH_NONE; @@ -1400,7 +1400,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NONE; + current_status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ @@ -1416,10 +1416,10 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_MISSION; + current_status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_NONE; + current_status->mission_switch = MISSION_SWITCH_LOITER; } else { current_status->mission_switch = MISSION_SWITCH_MISSION; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d9f037c27..9baf1a6af 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1372,18 +1372,6 @@ Sensors::rc_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* mode switch input */ - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - - /* land switch input */ - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - - /* assisted switch input */ - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - - /* mission switch input */ - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1394,14 +1382,26 @@ Sensors::rc_poll() } } + /* mode switch input */ if (_rc.function[MODE] >= 0) { manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } + + /* mission switch input */ if (_rc.function[MISSION] >= 0) { manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } + // if (_rc.function[OFFBOARD_MODE] >= 0) { // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); // } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ae3a24a1b..1a9dec5f5 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -99,11 +99,13 @@ typedef enum { typedef enum { RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_NORMAL, RETURN_SWITCH_RETURN } return_switch_pos_t; typedef enum { MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_LOITER, MISSION_SWITCH_MISSION } mission_switch_pos_t; -- cgit v1.2.3 From d35a1699078b6cb8a41d58bdbbd09164e5c0adf2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 31 Dec 2013 12:46:35 +0400 Subject: navigator: NAV_STATE_INIT removed, minor fixes --- src/modules/commander/commander.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 103 ++++++++++++------------- src/modules/uORB/topics/home_position.h | 2 +- src/modules/uORB/topics/vehicle_control_mode.h | 3 +- 4 files changed, 52 insertions(+), 58 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 03d3c02d1..1e5318121 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -605,7 +605,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&armed, 0, sizeof(armed)); status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_INIT; + status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4176cee1b..428887373 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -375,7 +375,8 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); /* Initialize state machine */ - myState = NAV_STATE_INIT; + myState = NAV_STATE_NONE; + start_none(); } Navigator::~Navigator() @@ -526,7 +527,10 @@ Navigator::task_main() /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = 0; + unsigned prevState = NAV_STATE_NONE; + bool pub_control_mode = true; + hrt_abstime mavlink_open_time = 0; + const hrt_abstime mavlink_open_period = 500000000; /* wakeup source(s) */ struct pollfd fds[7]; @@ -565,10 +569,16 @@ Navigator::task_main() perf_begin(_loop_perf); - /* only update vehicle status if it changed */ + if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time + mavlink_open_period) { + /* try to open the mavlink log device every once in a while */ + mavlink_open_time = hrt_abstime(); + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* vehicle status updated */ if (fds[6].revents & POLLIN) { - vehicle_status_update(); + pub_control_mode = true; /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO) { @@ -609,7 +619,6 @@ Navigator::task_main() _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; switch (_vstatus.set_nav_state) { - case NAV_STATE_INIT: case NAV_STATE_NONE: /* nothing to do */ break; @@ -632,8 +641,8 @@ Navigator::task_main() } } else { - /* on first switch to AUTO try mission, if none is available fallback to loiter instead */ - if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); } else { @@ -647,52 +656,46 @@ Navigator::task_main() /* not in AUTO */ dispatch(EVENT_NONE_REQUESTED); } - - /* XXX Hack to get mavlink output going, try opening the fd with 5Hz */ - if (_mavlink_fd < 0) { - /* try to open the mavlink log device every once in a while */ - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - } } - /* only update parameters if it changed */ + /* parameters updated */ if (fds[0].revents & POLLIN) { parameters_update(); /* note that these new parameters won't be in effect until a mission triplet is published again */ } - /* only update craft capabilities if they have changed */ + /* navigation capabilities updated */ if (fds[3].revents & POLLIN) { navigation_capabilities_update(); } + /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } + /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } + /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); // XXX check if home position really changed dispatch(EVENT_HOME_POSITION_CHANGED); } - /* only run controller if position changed */ + /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); /* only check if waypoint has been reached in Mission or RTL mode */ - if (mission_item_reached()) { - - if (_vstatus.main_state == MAIN_STATE_AUTO && - (myState == NAV_STATE_MISSION)) { - + if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { + if (mission_item_reached()) { /* advance by one mission item */ _mission.move_to_next(); @@ -702,18 +705,22 @@ Navigator::task_main() } else { dispatch(EVENT_MISSION_FINISHED); } - } else { - dispatch(EVENT_MISSION_FINISHED); } } } + + /* notify user about state changes */ if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); prevState = myState; + pub_control_mode = true; } - publish_control_mode(); + /* publish control mode if updated */ + if (pub_control_mode) { + publish_control_mode(); + } perf_end(_loop_perf); } @@ -767,9 +774,6 @@ Navigator::status() } switch (myState) { - case NAV_STATE_INIT: - warnx("State: Init"); - break; case NAV_STATE_NONE: warnx("State: None"); break; @@ -885,16 +889,6 @@ Navigator::fence_point(int argc, char *argv[]) StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_INIT */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_FINISHED */ {NO_ACTION, NAV_STATE_INIT}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_INIT}, - }, { /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, @@ -976,19 +970,21 @@ Navigator::start_loiter() _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.yaw = 0.0f; // TODO use current yaw sp here or set to undefined? + _mission_item_triplet.current.yaw = NAN; // NAN means to use current yaw get_loiter_item(&_mission_item_triplet.current); - float global_min_alt = _parameters.min_altitude + _home_pos.altitude; + // TODO use relative altitude to allow flying without global reference (?) + _mission_item_triplet.current.altitude_is_relative = false; + float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; /* Use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < global_min_alt) { - _mission_item_triplet.current.altitude = global_min_alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(global_min_alt - _global_pos.alt)); + if (_global_pos.alt < min_alt_amsl) { + _mission_item_triplet.current.altitude = min_alt_amsl; + mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); } else { _mission_item_triplet.current.altitude = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[navigator] loiter here"); + mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } publish_mission_item_triplet(); @@ -998,7 +994,7 @@ Navigator::start_loiter() void Navigator::start_mission() { - /* leave previous mission item as isas is */ + /* leave previous mission item as is as is */ int ret; bool onboard; @@ -1108,7 +1104,7 @@ Navigator::start_rtl() _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; - _mission_item_triplet.current.yaw = 0.0f; + _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number @@ -1212,8 +1208,7 @@ Navigator::mission_item_reached() } /* check if required yaw reached */ - float yaw_sp = _wrap_pi(_mission_item_triplet.current.yaw); - float yaw_err = _wrap_pi(yaw_sp - _global_pos.yaw); + float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; @@ -1223,7 +1218,7 @@ Navigator::mission_item_reached() if (_waypoint_position_reached /* && _waypoint_yaw_reached */) { /* XXX what about yaw? */ if (_time_first_inside_orbit == 0) { - /* XXX announcment? */ + /* XXX announcement? */ _time_first_inside_orbit = now; } @@ -1337,17 +1332,17 @@ Navigator::publish_control_mode() } bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { - if (fabsf(a.altitude_is_relative - b.altitude_is_relative) < FLT_EPSILON && - fabsf(a.lat - b.lat) < FLT_EPSILON && - fabsf(a.lon - b.lon) < FLT_EPSILON && + if (a.altitude_is_relative == b.altitude_is_relative && + fabs(a.lat - b.lat) < FLT_EPSILON && + fabs(a.lon - b.lon) < FLT_EPSILON && fabsf(a.altitude - b.altitude) < FLT_EPSILON && fabsf(a.yaw - b.yaw) < FLT_EPSILON && fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && - fabsf(a.loiter_direction - b.loiter_direction) < FLT_EPSILON && - fabsf(a.nav_cmd - b.nav_cmd) < FLT_EPSILON && + a.loiter_direction == b.loiter_direction && + a.nav_cmd == b.nav_cmd && fabsf(a.radius - b.radius) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && - fabsf(a.autocontinue - b.autocontinue) < FLT_EPSILON) { + a.autocontinue == b.autocontinue) { return true; } else { return false; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 1cf37e29c..3e2fee84e 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -59,7 +59,7 @@ struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - bool altitude_is_relative; + //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float altitude; /**< Altitude in meters */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5ce968f67..5efb3f5bc 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -62,8 +62,7 @@ */ typedef enum { - NAV_STATE_INIT = 0, - NAV_STATE_NONE, + NAV_STATE_NONE = 0, NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_MISSION_LOITER, -- cgit v1.2.3 From 3c72c9bf7fce882a1d9138eb83e5d5745e79c271 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 23:21:29 +0400 Subject: navigator: force takeoff if first mission item is not takeoff and vehicle landed --- src/modules/commander/commander_params.c | 2 - src/modules/navigator/navigator_main.cpp | 154 ++++++++++++++++++++++++------- src/modules/navigator/navigator_params.c | 4 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 123 insertions(+), 39 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 40d0386d5..691d3efcb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,8 +45,6 @@ #include #include -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.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/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 982ebefcc..757eb4690 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -170,6 +170,7 @@ private: bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; + bool _force_takeoff; MissionFeasibilityChecker missionFeasiblityChecker; @@ -179,13 +180,16 @@ private: float min_altitude; float loiter_radius; int onboard_mission_enabled; + float land_alt; + float rtl_alt; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; param_t loiter_radius; param_t onboard_mission_enabled; - + param_t land_alt; + param_t rtl_alt; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -204,6 +208,14 @@ private: */ static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; + enum RTLState { + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_LAND + }; + + enum RTLState _rtl_state; + /** * Update our local parameter cache. */ @@ -289,7 +301,12 @@ private: /** * Helper function to get a loiter item */ - void get_loiter_item(mission_item_s *new_loiter_position); + void get_loiter_item(mission_item_s *item); + + /** + * Helper function to get a takeoff item + */ + void get_takeoff_item(mission_item_s *item); /** * Publish a new mission item triplet for position controller @@ -361,13 +378,16 @@ Navigator::Navigator() : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _set_nav_state_timestamp(0) + _set_nav_state_timestamp(0), + _force_takeoff(false) { memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); + _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); + _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = false; @@ -418,6 +438,8 @@ Navigator::parameters_update() param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); + param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); } @@ -706,8 +728,14 @@ Navigator::task_main() if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (mission_item_reached()) { if (myState == NAV_STATE_MISSION) { - /* advance by one mission item */ - _mission.move_to_next(); + if (_force_takeoff) { + /* forced takeoff completed */ + _force_takeoff = false; + mavlink_log_info(_mavlink_fd, "[navigator] forced takeoff completed"); + } else { + /* advance by one mission item */ + _mission.move_to_next(); + } /* if no more mission items available send this to state machine and start loiter at the last WP */ if (_mission.current_mission_available()) { @@ -948,6 +976,7 @@ Navigator::start_none() _reset_loiter_pos = true; _rtl_done = false; + _force_takeoff = false; publish_mission_item_triplet(); } @@ -955,6 +984,8 @@ Navigator::start_none() void Navigator::start_loiter() { + _force_takeoff = false; + /* set loiter position if needed */ if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { _reset_loiter_pos = false; @@ -985,51 +1016,77 @@ Navigator::start_loiter() publish_mission_item_triplet(); } - void Navigator::start_mission() { + int ret; + bool onboard; + unsigned index; + // TODO set prev item to current position? _reset_loiter_pos = true; _rtl_done = false; + _force_takeoff = false; - int ret; - bool onboard; - unsigned index; + _mission_item_triplet.previous_valid = false; ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + add_home_pos_to_rtl(&_mission_item_triplet.current); - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); - } else { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); - } - } else { - /* since a mission is started without knowledge if there are more mission items available, this can fail */ - _mission_item_triplet.current_valid = false; - } + if (_mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF && _vstatus.condition_landed) { + /* if landed, takeoff first, if this not defined in mission */ + _force_takeoff = true; - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + /* move current mission item to next */ + memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.next_valid = true; - if (ret == OK) { + /* fill takeoff item */ + get_takeoff_item(&_mission_item_triplet.current); + if (_vstatus.is_rotary_wing) { + /* for VTOL use current position */ + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + } else { + /* for fixed wing use first waypoint position */ + // TODO what if first mission item has no lat/lon? + _mission_item_triplet.current.lat = _mission_item_triplet.next.lat; + _mission_item_triplet.current.lon = _mission_item_triplet.next.lon; + } + _mission_item_triplet.current.altitude = _global_pos.alt + _parameters.land_alt; + _mission_item_triplet.current.altitude_is_relative = false; + mavlink_log_info(_mavlink_fd, "[navigator] mission started, force takeoff to %.1fm", _mission_item_triplet.current.altitude); + } else { + /* normal mission start: mission starts with takeoff item or vehicle is already in air */ + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + if (ret == OK) { + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; + } else { + /* this will fail for the last WP */ + _mission_item_triplet.next_valid = false; + } + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); + } + } } else { - /* this will fail for the last WP */ + /* mission is started without knowledge if there are more mission, indicates bug in navigator */ + _mission_item_triplet.current_valid = false; _mission_item_triplet.next_valid = false; + mavlink_log_critical(_mavlink_fd, "[navigator] error: MISSION mode without mission defined"); } publish_mission_item_triplet(); } - - void Navigator::advance_mission() { @@ -1174,7 +1231,7 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - // current relative or AMSL altitude depending on _mission_item_triplet.current.altitude_is_relative + /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, @@ -1199,7 +1256,7 @@ Navigator::mission_item_reached() if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* require only altitude for takeoff */ - if (current_alt > _mission_item_triplet.current.altitude) + if (current_alt >= _mission_item_triplet.current.altitude) _waypoint_position_reached = true; } else { if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { @@ -1237,13 +1294,40 @@ Navigator::mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *new_loiter_position) +Navigator::get_loiter_item(struct mission_item_s *item) +{ + //item->altitude_is_relative + //item->lat + //item->lon + //item->altitude + item->yaw = NAN; + item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_direction = 1; + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->radius = 50.0f; // TODO: get rid of magic number + //item->time_inside + //item->pitch_min + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; + +} + +void +Navigator::get_takeoff_item(mission_item_s *item) { - new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - new_loiter_position->loiter_direction = 1; - new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_loiter_position->radius = 50.0f; // TODO: get rid of magic number - new_loiter_position->autocontinue = false; + //item->altitude_is_relative + //item->lat + //item->lon + //item->altitude + item->yaw = NAN; + item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_direction = 1; + item->nav_cmd = NAV_CMD_TAKEOFF; + item->radius = 50.0f; // TODO: get rid of magic number + item->time_inside = 0.0f; + item->pitch_min = 0.3; // TODO: get rid of magic number + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; } void diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index b9d887379..63601ce6d 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,4 +53,6 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); \ No newline at end of file +PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 10.0f); // default TAKEOFF altitude, slow descend from this altitude when landing in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 10.0f); // min altitude for going home in RTL mode diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9b4250115..95bceb8bb 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -83,7 +83,7 @@ struct mission_item_s double lat; /**< latitude in degrees */ double lon; /**< longitude in degrees */ float altitude; /**< altitude in meters */ - float yaw; /**< in radians NED -PI..+PI */ + float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< navigation command */ -- cgit v1.2.3 From 5a1b39a17215103b8f8181d1ba5e44257abc5b34 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 00:15:34 +0100 Subject: RTL on RC failsafe --- src/modules/commander/commander.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dbfca38b1..cedcbb3fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1133,6 +1133,14 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; + if (status.main_state != MAIN_STATE_AUTO) { + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode"); + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } } } } -- cgit v1.2.3 From b175937b5ffab87e8951c620d7673582341f98b4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 18 Jan 2014 14:25:24 +0100 Subject: navigator, commander: RTL and RC failsafe fixes --- src/modules/commander/commander.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 38 ++++++++++++++++++++++++-------- src/modules/navigator/navigator_params.c | 2 +- 3 files changed, 31 insertions(+), 11 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cedcbb3fb..6a668fcd7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1133,7 +1133,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - if (status.main_state != MAIN_STATE_AUTO) { + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode"); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bb7520a03..4bc05d107 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -180,6 +180,8 @@ private: uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ + char *nav_states_str[NAV_STATE_MAX]; + struct { float min_altitude; float acceptance_radius; @@ -415,6 +417,12 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); + nav_states_str[0] = "NONE"; + nav_states_str[1] = "READY"; + nav_states_str[2] = "LOITER"; + nav_states_str[3] = "MISSION"; + nav_states_str[4] = "RTL"; + /* Initialize state machine */ myState = NAV_STATE_NONE; start_none(); @@ -788,7 +796,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); prevState = myState; pub_control_mode = true; } @@ -881,7 +889,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { }, { /* STATE_READY */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, @@ -917,7 +925,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, }; @@ -945,8 +953,8 @@ Navigator::start_ready() _reset_loiter_pos = true; _do_takeoff = false; - // TODO check if (_rtl_state != RTL_STATE_LAND) { + /* allow RTL if landed not at home */ _rtl_state = RTL_STATE_NONE; } @@ -977,6 +985,11 @@ Navigator::start_loiter() _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); } + + if (_rtl_state == RTL_STATE_LAND) { + /* if RTL landing was interrupted, avoid landing from MIN_ALT on next RTL */ + _rtl_state = RTL_STATE_DESCEND; + } } _mission_item_triplet.previous_valid = false; @@ -1103,11 +1116,19 @@ Navigator::advance_mission() void Navigator::start_rtl() { - _reset_loiter_pos = true; _do_takeoff = false; - if (_rtl_state == RTL_STATE_NONE) - _rtl_state = RTL_STATE_CLIMB; - + if (_rtl_state == RTL_STATE_NONE) { + if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { + _rtl_state = RTL_STATE_CLIMB; + } else { + _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.altitude = _global_pos.alt; + } + } + } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); } @@ -1150,7 +1171,6 @@ Navigator::set_rtl_item() _mission_item_triplet.current_valid = true; _mission_item_triplet.next_valid = false; - _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; // don't change altitude setpoint diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 8df47fc3b..617b2ec31 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,6 +55,6 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); -PARAM_DEFINE_FLOAT(NAV_TAAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -- cgit v1.2.3 From eb9fd154fef4f01c6eda1bb87ee7ea87c6c04133 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 21 Jan 2014 15:26:51 +0100 Subject: commander: more robust RC failsafe, but still hotfix, needs to be rewritten --- src/modules/commander/commander.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6a668fcd7..47053838c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1133,12 +1133,17 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { + res = main_state_transition(&status, MAIN_STATE_SEATBELT); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); } } } -- cgit v1.2.3 From 1dc9785083e1ed7a7db27466fbe9f61f6bb277f7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 16:52:35 +0100 Subject: OFFBOARD switch added, WIP --- src/modules/commander/commander.cpp | 11 +++++++++ src/modules/sensors/sensor_params.c | 3 +-- src/modules/sensors/sensors.cpp | 29 ++++++++++------------- src/modules/uORB/topics/manual_control_setpoint.h | 4 +--- src/modules/uORB/topics/vehicle_status.h | 8 +++++++ 5 files changed, 34 insertions(+), 21 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47053838c..e2b6ad1ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1410,6 +1410,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { current_status->mission_switch = MISSION_SWITCH_MISSION; } + + /* offboard switch */ + if (!isfinite(sp_man->offboard_switch)) { + current_status->offboard_switch = OFFBOARD_SWITCH_NONE; + + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + + } else { + current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; + } } transition_result_t diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bbc84ef93..eb771382d 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -390,8 +390,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 6); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); - -//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5e8c5edbc..f87f2f282 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,8 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; - -// int rc_map_offboard_ctrl_mode_sw; + int rc_map_offboard_sw; int rc_map_flaps; @@ -296,8 +295,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; - -// param_t rc_map_offboard_ctrl_mode_sw; + param_t rc_map_offboard_sw; param_t rc_map_flaps; @@ -515,8 +513,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); - -// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -669,14 +666,14 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("Failed getting offboard sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } -// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { -// warnx("Failed getting offboard control mode sw chan 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)); @@ -700,11 +697,10 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; -// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; @@ -1289,7 +1285,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; + manual_control.offboard_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1432,9 +1428,10 @@ Sensors::rc_poll() manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* offboard switch input */ + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.offboard_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..ab0c7a411 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,15 +60,13 @@ struct manual_control_setpoint_s { 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 */ + float offboard_switch; /**< offboard switch (optional): offboard, onboard */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..d74b696bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_OFFBOARD, } main_state_t; typedef enum { @@ -109,6 +110,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + OFFBOARD_SWITCH_NONE = 0, + OFFBOARD_SWITCH_OFFBOARD, + OFFBOARD_SWITCH_ONBOARD +} offboard_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -184,6 +191,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + offboard_switch_pos_t offboard_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ -- cgit v1.2.3 From 37e28e7fa82a965967abba6650ba24a31aba10d3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 17:04:07 +0100 Subject: commander: OFFBOARD mode added, WIP --- src/modules/commander/commander.cpp | 80 ++++++++++++++------------ src/modules/commander/state_machine_helper.cpp | 6 ++ 2 files changed, 49 insertions(+), 37 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2b6ad1ca..dfcb1cc9f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1429,56 +1429,62 @@ 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); - // TRANSITION_DENIED is not possible here - break; + if (current_status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + /* offboard switch overrides main switch */ + res = main_state_transition(current_status, MAIN_STATE_OFFBOARD); - case MODE_SWITCH_ASSISTED: - if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY); + } else { + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY); - // else fallback to SEATBELT - print_reject_mode("EASY"); - } + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT + print_reject_mode("EASY"); + } - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this mode + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); - if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + 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); - // TRANSITION_DENIED is not possible here - break; + 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); + // TRANSITION_DENIED is not possible here + break; - case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state - // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + // else fallback to SEATBELT (EASY likely will not work too) + 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 + 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); - // TRANSITION_DENIED is not possible here - break; + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL); + // TRANSITION_DENIED is not possible here + break; - default: - break; + default: + break; + } } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..e5599e1c3 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -262,6 +262,12 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m ret = TRANSITION_CHANGED; } + break; + + case MAIN_STATE_OFFBOARD: + + ret = TRANSITION_CHANGED; + break; } -- cgit v1.2.3 From f069fe9f61d24b5a6cc66d60afd3f8b5e568824a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 22 Jan 2014 19:25:14 +0100 Subject: OFFBOARD mode support in commander, mavlink, mc_att_control, WIP --- src/modules/commander/commander.cpp | 66 +++++++++------ src/modules/commander/px4_custom_mode.h | 1 + src/modules/mavlink/mavlink.c | 2 + src/modules/mc_att_control/mc_att_control_main.cpp | 99 ++++++++++++---------- src/modules/navigator/navigator_main.cpp | 16 +++- 5 files changed, 110 insertions(+), 74 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfcb1cc9f..7b69f2b76 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -411,42 +411,52 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - /* use autopilot-specific mode */ - if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); - - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } + if (status->rc_signal_lost) { + /* allow mode switching by command only if no RC signal */ + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else { - /* use base mode */ - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_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_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { /* 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); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); + } + + } 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); + } } } + } else { + mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); } + if (main_res == TRANSITION_CHANGED) ret = true; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..4843e72c3 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_SEATBELT, PX4_CUSTOM_MAIN_MODE_EASY, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_OFFBOARD, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f8091716..70229558c 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -231,6 +231,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + } else if (v_status.main_state == MAIN_STATE_OFFBOARD) { + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 245ac024b..c4c6c0138 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -480,64 +480,73 @@ MulticopterAttitudeControl::task_main() float yaw_sp_move_rate = 0.0f; bool publish_att_sp = false; - /* define which input is the dominating control input */ - if (_control_mode.flag_control_manual_enabled) { - /* manual input */ - 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_offboard_enabled) { + /* offboard control */ + // TODO set _att_sp or _rates_sp here depending on offboard control mode + // TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode + // but it's not very beautiful, need to think how to do it better - if (!_arming.armed) { - /* reset yaw setpoint when disarmed */ - reset_yaw_sp = true; - } + } else { + /* onboard control */ + /* define which input is the dominating control input */ + if (_control_mode.flag_control_manual_enabled) { + /* manual input */ + 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_attitude_enabled) { - /* control attitude, update attitude setpoint depending on mode */ + if (!_arming.armed) { + /* reset yaw setpoint when disarmed */ + reset_yaw_sp = true; + } - if (_att_sp.thrust < 0.1f) { - // TODO - //if (_status.condition_landed) { - /* reset yaw setpoint if on ground */ - // reset_yaw_sp = true; - //} - } else { - if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { - /* move yaw setpoint */ - yaw_sp_move_rate = _manual.yaw; - _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + if (_control_mode.flag_control_attitude_enabled) { + /* control attitude, update attitude setpoint depending on mode */ + + if (_att_sp.thrust < 0.1f) { + // TODO + //if (_status.condition_landed) { + /* reset yaw setpoint if on ground */ + // reset_yaw_sp = true; + //} + } else { + if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) { + /* move yaw setpoint */ + yaw_sp_move_rate = _manual.yaw; + _att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt); + _att_sp.R_valid = false; + publish_att_sp = true; + } + } + + /* reset yaw setpint to current position if needed */ + if (reset_yaw_sp) { + reset_yaw_sp = false; + _att_sp.yaw_body = _att.yaw; _att_sp.R_valid = false; publish_att_sp = true; } - } - /* reset yaw setpint to current position if needed */ - if (reset_yaw_sp) { - reset_yaw_sp = false; - _att_sp.yaw_body = _att.yaw; - _att_sp.R_valid = false; - publish_att_sp = 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; + _att_sp.R_valid = false; + publish_att_sp = 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; - _att_sp.R_valid = false; - publish_att_sp = true; + } else { + /* manual rate inputs (ACRO) */ + // TODO + /* reset yaw setpoint after ACRO */ + reset_yaw_sp = true; } } else { - /* manual rate inputs (ACRO) */ - // TODO - /* reset yaw setpoint after ACRO */ + /* reset yaw setpoint after non-manual control */ reset_yaw_sp = true; } - - } else { - /* reset yaw setpoint after non-manual control */ - reset_yaw_sp = true; } if (_att_sp.R_valid) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ca5735509..7285e243e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1463,11 +1463,11 @@ Navigator::publish_control_mode() _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_flighttermination_enabled = false; switch (_vstatus.main_state) { case MAIN_STATE_MANUAL: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; @@ -1478,6 +1478,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_SEATBELT: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1488,6 +1489,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_EASY: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = true; _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1498,6 +1500,7 @@ Navigator::publish_control_mode() break; case MAIN_STATE_AUTO: + _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1517,6 +1520,17 @@ Navigator::publish_control_mode() } break; + case MAIN_STATE_OFFBOARD: + _control_mode.flag_control_offboard_enabled = true; + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + default: break; } -- cgit v1.2.3 From 0877fcba0312cf859b2a8ccd9eb3b6d56ecaf7e4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:52:02 +0100 Subject: offboard setpoint support --- src/modules/commander/commander.cpp | 213 ++++++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 48 +++++-- src/modules/navigator/navigator_main.cpp | 57 ++++++++- 3 files changed, 215 insertions(+), 103 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7b69f2b76..fa815bdfe 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define OFFBOARD_TIMEOUT 200000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -699,9 +700,9 @@ int commander_thread_main(int argc, char *argv[]) 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)); + int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -815,10 +816,10 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &updated); + orb_check(offboard_sp_sub, &updated); if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); + orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); } orb_check(sensor_sub, &updated); @@ -1040,124 +1041,162 @@ 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 (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; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); - status_changed = true; + /* start RC input check */ + 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; + 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, "#audio: RC signal regained"); - status_changed = true; - } + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + status_changed = true; } + } - status.rc_signal_lost = false; - - transition_result_t res; // store all transitions results here + status.rc_signal_lost = false; - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + transition_result_t res; // store all transitions results here - /* 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - 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; + /* 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.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - } else { - stick_off_counter++; - } + 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++; } - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - 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 { + stick_off_counter = 0; + } - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); - stick_on_counter = 0; + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - stick_on_counter++; + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else { stick_on_counter = 0; + + } else { + stick_on_counter++; } - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + } else { + stick_on_counter = 0; + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - } 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, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); + } 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, "#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 */ + check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ - res = check_main_state_machine(&status); + /* evaluate the main state machine */ + res = check_main_state_machine(&status); - if (res == TRANSITION_CHANGED) { - //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); - tune_positive(); + if (res == TRANSITION_CHANGED) { + //mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); - } 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, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } 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, "#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, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { + res = main_state_transition(&status, MAIN_STATE_SEATBELT); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); + } } + } + } + + /* check offboard signal */ + if (hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (!status.offboard_control_signal_found_once) { + status.offboard_control_signal_found_once = true; + mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); + status_changed = true; } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; + if (status.offboard_control_signal_lost) { + mavlink_log_info(mavlink_fd, "[cmd] offboard signal regained"); status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); + } + + status.offboard_control_signal_lost = false; + + if (status.main_state == MAIN_STATE_OFFBOARD) { + if (offboard_sp.armed && !armed.armed) { + if (!safety.safety_switch_available || safety.safety_off) { + transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); + mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } + } else if (!offboard_sp.armed && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); + } } } + + } else { + if (!status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST"); + status.offboard_control_signal_lost = true; + status_changed = true; + } } /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ @@ -1264,7 +1303,7 @@ int commander_thread_main(int argc, char *argv[]) led_deinit(); buzzer_deinit(); close(sp_man_sub); - close(sp_offboard_sub); + close(offboard_sp_sub); close(local_position_sub); close(global_position_sub); close(gps_sub); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f9c718d2..62597d91a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -97,6 +97,7 @@ static mavlink_status_t status; static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; +static struct vehicle_attitude_setpoint_s att_sp; struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; @@ -125,6 +126,7 @@ static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; +static orb_advert_t att_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; @@ -266,8 +268,8 @@ handle_message(mavlink_message_t *msg) /* Handle quadrotor motor setpoints */ if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_swarm_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_swarm_setpoint); if (mavlink_system.sysid < 4) { @@ -281,7 +283,7 @@ handle_message(mavlink_message_t *msg) uint8_t ml_mode = 0; bool ml_armed = false; - switch (quad_motors_setpoint.mode) { + switch (quad_swarm_setpoint.mode) { case 0: ml_armed = false; break; @@ -307,12 +309,12 @@ handle_message(mavlink_message_t *msg) break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_swarm_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_swarm_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_swarm_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + if (quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -326,9 +328,37 @@ handle_message(mavlink_message_t *msg) offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { - /* Publish */ + /* publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } + + if (v_status.main_state == MAIN_STATE_OFFBOARD) { + /* in offboard mode also publish setpoint directly */ + switch (offboard_control_sp.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; + att_sp.R_valid = false; + att_sp.q_d_valid = false; + att_sp.q_e_valid = false; + att_sp.roll_reset_integral = false; + + /* check if topic has to be advertised */ + if (att_sp_pub <= 0) { + att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + break; + default: + break; + } + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7285e243e..f77c48207 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -149,8 +150,9 @@ private: int _vstatus_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ + int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _offboard_sub; /**< offboard control setpoint subscription */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ @@ -162,6 +164,7 @@ private: struct home_position_s _home_pos; /**< home position for RTL */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ + struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -383,6 +386,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _onboard_mission_sub(-1), _capabilities_sub(-1), + _offboard_sub(-1), /* publications */ _triplet_pub(-1), @@ -422,6 +426,7 @@ Navigator::Navigator() : memset(&_mission_item_triplet.next, 0, sizeof(struct mission_item_s)); memset(&_mission_result, 0, sizeof(struct mission_result_s)); + memset(&_offboard, 0, sizeof(_offboard)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; @@ -591,6 +596,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -1523,12 +1529,49 @@ Navigator::publish_control_mode() case MAIN_STATE_OFFBOARD: _control_mode.flag_control_offboard_enabled = true; _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + + switch (_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + default: + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + } break; default: -- cgit v1.2.3 From 6ed6c5bb1fe6cce2c2c784d5f8145c0aa03fabe7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 23 Jan 2014 17:56:38 +0100 Subject: cammander, navigator: code style fixed --- src/modules/commander/commander.cpp | 26 +- src/modules/commander/state_machine_helper.cpp | 56 ++-- src/modules/navigator/navigator_main.cpp | 352 +++++++++++++++---------- 3 files changed, 258 insertions(+), 176 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fa815bdfe..bc8278bb7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -372,6 +372,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -406,6 +407,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -454,6 +456,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + } else { mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd"); } @@ -497,8 +500,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -519,7 +523,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO @@ -891,6 +895,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; @@ -1066,9 +1071,9 @@ int commander_thread_main(int argc, char *argv[]) /* 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + 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 */ @@ -1086,7 +1091,7 @@ 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 && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1143,14 +1148,18 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } + if (status.main_state != MAIN_STATE_AUTO && armed.armed) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); status.set_nav_state = NAV_STATE_RTL; status.set_nav_state_timestamp = hrt_absolute_time(); + } else if (status.main_state != MAIN_STATE_SEATBELT) { res = main_state_transition(&status, MAIN_STATE_SEATBELT); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); } @@ -1178,13 +1187,16 @@ int commander_thread_main(int argc, char *argv[]) if (offboard_sp.armed && !armed.armed) { if (!safety.safety_switch_available || safety.safety_off) { transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); } } + } else if (!offboard_sp.armed && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); } @@ -1202,9 +1214,11 @@ int commander_thread_main(int argc, char *argv[]) /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); + if (flighttermination_res == TRANSITION_CHANGED) { tune_positive(); } + } else { flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e5599e1c3..4dd63b5e1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool flighttermination_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_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -238,8 +239,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need at minimum altitude estimate */ if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } @@ -249,7 +250,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need at minimum local position estimate */ if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { + current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -373,35 +374,36 @@ transition_result_t flighttermination_state_transition(struct vehicle_status_s * { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; + /* only check transition if the new state is actually different from the current one */ + if (new_flighttermination_state == status->flighttermination_state) { + ret = TRANSITION_NOT_CHANGED; - } else { + } else { - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; + switch (new_flighttermination_state) { + case FLIGHTTERMINATION_STATE_ON: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; + warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); + break; - default: - break; - } + case FLIGHTTERMINATION_STATE_OFF: + ret = TRANSITION_CHANGED; + status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; + break; - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + default: + break; } - return ret; + if (ret == TRANSITION_CHANGED) { + flighttermination_state_changed = true; + // TODO + //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; + } + } + + return ret; } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index f77c48207..0e317d784 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -167,12 +167,12 @@ private: struct offboard_control_setpoint_s _offboard; /**< offboard control setpoint, to get desired offboard control mode only */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -369,7 +369,7 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -508,16 +508,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -534,6 +538,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -576,11 +581,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -597,7 +604,7 @@ Navigator::task_main() _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); _offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -665,8 +672,9 @@ Navigator::task_main() /* Evaluate state machine from commander and set the navigator mode accordingly */ if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { + (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ @@ -674,21 +682,27 @@ Navigator::task_main() if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + stick_mode = true; + } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { /* switch to mission only if available */ if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + stick_mode = true; } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); @@ -714,15 +728,18 @@ Navigator::task_main() case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + break; case NAV_STATE_RTL: if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + break; default: @@ -735,6 +752,7 @@ Navigator::task_main() if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } @@ -762,7 +780,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -783,6 +801,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -791,15 +810,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -849,48 +868,55 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); + (double)_global_pos.alt, (double)_global_pos.relative_alt); warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -899,8 +925,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ + { + /* STATE_READY */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, @@ -910,7 +936,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, @@ -919,8 +945,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -929,8 +955,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -993,6 +1019,7 @@ Navigator::start_loiter() if (_global_pos.alt < min_alt_amsl) { _mission_item_triplet.current.altitude = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { _mission_item_triplet.current.altitude = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); @@ -1043,27 +1070,28 @@ Navigator::set_mission_item() add_home_pos_to_rtl(&_mission_item_triplet.current); if (_mission_item_triplet.current.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item_triplet.current.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; /* calculate desired takeoff altitude AMSL */ float takeoff_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) takeoff_alt_amsl += _home_pos.altitude; @@ -1089,6 +1117,7 @@ Navigator::set_mission_item() _mission_item_triplet.current.altitude = takeoff_alt_amsl; _mission_item_triplet.current.altitude_is_relative = false; } + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; @@ -1097,13 +1126,16 @@ Navigator::set_mission_item() if (_do_takeoff) { mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_triplet.current_valid = false; @@ -1116,6 +1148,7 @@ Navigator::set_mission_item() if (ret == OK) { add_home_pos_to_rtl(&_mission_item_triplet.next); _mission_item_triplet.next_valid = true; + } else { /* this will fail for the last WP */ _mission_item_triplet.next_valid = false; @@ -1129,17 +1162,21 @@ void Navigator::start_rtl() { _do_takeoff = false; + if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; + } else { _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.altitude = _global_pos.alt; } } } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); @@ -1150,107 +1187,112 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; - if (_vstatus.condition_landed) - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + + if (_vstatus.condition_landed) + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _mission_item_triplet.current.altitude = climb_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; - _mission_item_triplet.current.altitude = climb_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); - break; - } case RTL_STATE_RETURN: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + // don't change altitude setpoint + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; - - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - // don't change altitude setpoint - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } case RTL_STATE_DESCEND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; - float descend_alt = _home_pos.altitude + _parameters.land_alt; + float descend_alt = _home_pos.altitude + _parameters.land_alt; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = descend_alt; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = descend_alt; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm", descend_alt - _home_pos.altitude); - break; - } case RTL_STATE_LAND: { - memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); - _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; + memcpy(&_mission_item_triplet.previous, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; - _mission_item_triplet.current_valid = true; - _mission_item_triplet.next_valid = false; + _mission_item_triplet.current_valid = true; + _mission_item_triplet.next_valid = false; + + _mission_item_triplet.current.altitude_is_relative = false; + _mission_item_triplet.current.lat = _home_pos.lat; + _mission_item_triplet.current.lon = _home_pos.lon; + _mission_item_triplet.current.altitude = _home_pos.altitude; + _mission_item_triplet.current.yaw = NAN; + _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; + _mission_item_triplet.current.loiter_direction = 1; + _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; + _mission_item_triplet.current.time_inside = 0.0f; + _mission_item_triplet.current.pitch_min = 0.0f; + _mission_item_triplet.current.autocontinue = true; + _mission_item_triplet.current.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - _mission_item_triplet.current.altitude_is_relative = false; - _mission_item_triplet.current.lat = _home_pos.lat; - _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude; - _mission_item_triplet.current.yaw = NAN; - _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.loiter_direction = 1; - _mission_item_triplet.current.nav_cmd = NAV_CMD_LAND; - _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; - _mission_item_triplet.current.time_inside = 0.0f; - _mission_item_triplet.current.pitch_min = 0.0f; - _mission_item_triplet.current.autocontinue = true; - _mission_item_triplet.current.origin = ORIGIN_ONBOARD; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } publish_mission_item_triplet(); @@ -1267,6 +1309,7 @@ Navigator::check_mission_item_reached() if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; + } else { /* For fw there is currently no landing detector: * make sure control is not stopped when overshooting the landing waypoint */ @@ -1281,7 +1324,7 @@ Navigator::check_mission_item_reached() _mission_item_triplet.current.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); @@ -1301,18 +1344,20 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) _mission_item_triplet.current.altitude += _home_pos.altitude; dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, - &dist_xy, &dist_z); + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1324,9 +1369,11 @@ Navigator::check_mission_item_reached() if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item_triplet.current.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1336,20 +1383,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; + if (_mission_item_triplet.current.time_inside > 0.01f) { mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item_triplet.current.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item_triplet.current.time_inside * 1e6) - || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + || _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1362,6 +1411,7 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); @@ -1369,23 +1419,28 @@ Navigator::on_mission_item_reached() if (_mission.current_mission_available()) { set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1508,6 +1563,7 @@ Navigator::publish_control_mode() case MAIN_STATE_AUTO: _control_mode.flag_control_offboard_enabled = false; _control_mode.flag_control_manual_enabled = false; + if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; @@ -1516,6 +1572,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + } else { _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1524,6 +1581,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } + break; case MAIN_STATE_OFFBOARD: @@ -1539,6 +1597,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1547,6 +1606,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1555,6 +1615,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = true; break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1563,6 +1624,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; break; + default: _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1572,6 +1634,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; break; } + break; default: @@ -1591,7 +1654,8 @@ Navigator::publish_control_mode() } } -bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) { +bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const struct mission_item_s b) +{ if (a.altitude_is_relative == b.altitude_is_relative && fabs(a.lat - b.lat) < FLT_EPSILON && fabs(a.lon - b.lon) < FLT_EPSILON && @@ -1604,6 +1668,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && a.autocontinue == b.autocontinue) { return true; + } else { return false; } @@ -1664,8 +1729,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); -- cgit v1.2.3 From 58792c5ca6e42bc251dd3c92b0e79217ff5d5403 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 24 Jan 2014 00:06:10 +0100 Subject: Use double for lat/lon in vehicle_global_position topic, use filed names lat, lon, alt, vel_n, vel_e, vel_d for global positions --- src/drivers/frsky_telemetry/frsky_data.c | 6 ++-- src/examples/fixedwing_control/main.c | 6 +--- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 7 ++-- .../attitude_estimator_ekf_main.cpp | 6 ++-- src/modules/commander/commander.cpp | 10 +++--- src/modules/fixedwing_backside/fixedwing.cpp | 8 ++--- .../fixedwing_pos_control_main.c | 2 +- src/modules/fw_att_control/fw_att_control_main.cpp | 6 ++-- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 6 ++-- src/modules/mavlink/orb_listener.c | 21 ++++++------ src/modules/navigator/navigator_main.cpp | 40 +++++++++++----------- .../position_estimator_inav_main.c | 14 +++----- src/modules/sdlog2/sdlog2.c | 10 +++--- src/modules/uORB/topics/home_position.h | 2 +- src/modules/uORB/topics/vehicle_global_position.h | 17 +++++---- 16 files changed, 76 insertions(+), 87 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index 63b2d2d29..e201ecbb3 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -230,11 +230,11 @@ void frsky_send_frame2(int uart) struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - lat = frsky_format_gps(abs(global_pos.lat) / 10000000.0f); + lat = frsky_format_gps(abs(global_pos.lat)); lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; - lon = frsky_format_gps(abs(global_pos.lon) / 10000000.0f); + lon = frsky_format_gps(abs(global_pos.lon)); lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy) + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) * 25.0f / 46.0f; alt = global_pos.alt; sec = tm_gps->tm_sec; diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index b286e0007..067d77364 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -181,11 +181,7 @@ void control_heading(const struct vehicle_global_position_s *pos, const struct v * Calculate heading error of current position to desired position */ - /* - * PX4 uses 1e7 scaled integers to represent global coordinates for max resolution, - * so they need to be scaled by 1e7 and converted to IEEE double precision floating point. - */ - float bearing = get_bearing_to_next_waypoint(pos->lat/1e7d, pos->lon/1e7d, sp->lat/1e7d, sp->lon/1e7d); + float bearing = get_bearing_to_next_waypoint(pos->lat, pos->lon, sp->lat, sp->lon); /* calculate heading error */ float yaw_err = att->yaw - bearing; diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index aca3fe7b6..8e88130e1 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -318,10 +318,9 @@ void KalmanNav::updatePublications() _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); _pos.alt = float(alt); - _pos.relative_alt = float(alt); // TODO, make relative - _pos.vx = vN; - _pos.vy = vE; - _pos.vz = vD; + _pos.vel_n = vN; + _pos.vel_e = vE; + _pos.vel_d = vD; _pos.yaw = psi; // local position publication diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6a1bec153..66ec20b95 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -414,9 +414,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; - vel(0) = global_pos.vx; - vel(1) = global_pos.vy; - vel(2) = global_pos.vz; + vel(0) = global_pos.vel_n; + vel(1) = global_pos.vel_e; + vel(2) = global_pos.vel_d; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47053838c..d51bb63ff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1009,12 +1009,12 @@ int commander_thread_main(int argc, char *argv[]) /* copy position data to uORB home message, store it locally as well */ - home.lat = (double)global_position.lat / 1e7d; - home.lon = (double)global_position.lon / 1e7d; - home.altitude = (float)global_position.alt; + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.altitude); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.altitude); + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 108e9896d..f7c0b6148 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -174,9 +174,9 @@ void BlockMultiModeBacksideAutopilot::update() // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // limit velocity command between min/max velocity float vCmd = _vLimit.update(_vCmd.get()); @@ -236,9 +236,9 @@ void BlockMultiModeBacksideAutopilot::update() // for the purpose of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( - _pos.vx * _pos.vx + + _pos.vel_n * _pos.vel_n + _pos.vy * _pos.vy + - _pos.vz * _pos.vz)); + _pos.vel_d * _pos.vel_d)); // pitch channel -> rate of climb // TODO, might want to put a gain on this, otherwise commanding diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 73df3fb9e..888dd0942 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -299,7 +299,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); start_pos = global_pos; //for now using the current position as the startpoint (= approx. last waypoint because the setpoint switch occurs at the waypoint) global_sp_updated_set_once = true; - psi_track = get_bearing_to_next_waypoint((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, + psi_track = get_bearing_to_next_waypoint(global_pos.lat, global_pos.lon, (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); printf("next wp direction: %0.4f\n", (double)psi_track); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index e49b3c140..dc2196de6 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -704,9 +704,9 @@ FixedwingAttitudeControl::task_main() float speed_body_v = 0.0f; float speed_body_w = 0.0f; if(_att.R_valid) { - speed_body_u = _att.R[0][0] * _global_pos.vx + _att.R[1][0] * _global_pos.vy + _att.R[2][0] * _global_pos.vz; - speed_body_v = _att.R[0][1] * _global_pos.vx + _att.R[1][1] * _global_pos.vy + _att.R[2][1] * _global_pos.vz; - speed_body_w = _att.R[0][2] * _global_pos.vx + _att.R[1][2] * _global_pos.vy + _att.R[2][2] * _global_pos.vz; + speed_body_u = _att.R[0][0] * _global_pos.vel_n + _att.R[1][0] * _global_pos.vel_e + _att.R[2][0] * _global_pos.vel_d; + speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; + speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { warnx("Did not get a valid R\n"); } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index f3d688646..a62b53221 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1262,7 +1262,7 @@ FixedwingPositionControl::task_main() vehicle_airspeed_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy); + math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f); /* diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 4f9c718d2..7c23488d7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -638,9 +638,9 @@ handle_message(mavlink_message_t *msg) hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; + hil_global_pos.vel_n = hil_state.vx / 100.0f; + hil_global_pos.vel_e = hil_state.vy / 100.0f; + hil_global_pos.vel_d = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 0b8ac6d3d..7f6237535 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct home_position_s home; struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct vehicle_control_mode_s control_mode; @@ -247,10 +248,10 @@ l_vehicle_attitude(const struct listener *l) hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { last_sent_vfr = t; - float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + float groundspeed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vel_d); } /* send quaternion values if it exists */ @@ -380,13 +381,13 @@ l_global_position(const struct listener *l) mavlink_msg_global_position_int_send(MAVLINK_COMM_0, global_pos.timestamp / 1000, - global_pos.lat, - global_pos.lon, + global_pos.lat * 1e7, + global_pos.lon * 1e7, global_pos.alt * 1000.0f, - global_pos.relative_alt * 1000.0f, - global_pos.vx * 100.0f, - global_pos.vy * 100.0f, - global_pos.vz * 100.0f, + (global_pos.alt - home.alt) * 1000.0f, + global_pos.vel_n * 100.0f, + global_pos.vel_e * 100.0f, + global_pos.vel_d * 100.0f, _wrap_2pi(global_pos.yaw) * M_RAD_TO_DEG_F * 100.0f); } @@ -657,11 +658,9 @@ l_optical_flow(const struct listener *l) void l_home(const struct listener *l) { - struct home_position_s home; - orb_copy(ORB_ID(home_position), mavlink_subs.home_sub, &home); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.altitude)*1e3f); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, (int32_t)(home.lat*1e7d), (int32_t)(home.lon*1e7d), (int32_t)(home.alt)*1e3f); } void diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ba51b024f..cfcc886b6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -833,11 +833,11 @@ Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); if (_global_pos.valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon / 1e7d, _global_pos.lat / 1e7d); + warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)_global_pos.relative_alt); - warnx("Ground velocity in m/s, x %5.5f, y %5.5f, z %5.5f", - (double)_global_pos.vx, (double)_global_pos.vy, (double)_global_pos.vz); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } if (_fence_valid) { @@ -964,11 +964,11 @@ Navigator::start_loiter() if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { _reset_loiter_pos = false; - _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; + float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { @@ -1063,8 +1063,8 @@ Navigator::set_mission_item() /* set current setpoint to takeoff */ - _pos_sp_triplet.current.lat = (double)_global_pos.lat / 1e7d; - _pos_sp_triplet.current.lon = (double)_global_pos.lon / 1e7d; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.alt = takeoff_alt_amsl; _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; @@ -1111,7 +1111,7 @@ Navigator::start_rtl() { _do_takeoff = false; if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.altitude + _parameters.rtl_alt) { + if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; } else { _rtl_state = RTL_STATE_RETURN; @@ -1133,15 +1133,15 @@ Navigator::set_rtl_item() case RTL_STATE_CLIMB: { memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - float climb_alt = _home_pos.altitude + _parameters.rtl_alt; + float climb_alt = _home_pos.alt + _parameters.rtl_alt; if (_vstatus.condition_landed) { climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); } _mission_item_valid = true; - _mission_item.lat = (double)_global_pos.lat / 1e7d; - _mission_item.lon = (double)_global_pos.lon / 1e7d; + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = climb_alt; _mission_item.yaw = NAN; @@ -1158,7 +1158,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); break; } case RTL_STATE_RETURN: { @@ -1194,7 +1194,7 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.altitude + _parameters.land_alt; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; @@ -1220,7 +1220,7 @@ Navigator::set_rtl_item() _mission_item.lat = _home_pos.lat; _mission_item.lon = _home_pos.lon; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.altitude; + _mission_item.altitude = _home_pos.alt; _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; @@ -1256,11 +1256,11 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ /* set home position for RTL item */ sp->lat = _home_pos.lat; sp->lon = _home_pos.lon; - sp->alt = _home_pos.altitude + _parameters.rtl_alt; + sp->alt = _home_pos.alt + _parameters.rtl_alt; } else { sp->lat = item->lat; sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.altitude : item->altitude; + sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; } sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; @@ -1325,10 +1325,10 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.altitude; + wp_alt_amsl += _home_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); if (_do_takeoff) { 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 02fa6a8f2..af04bb0bc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -840,19 +840,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global) { 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 * 1e7d); - global_pos.lon = (int32_t)(est_lon * 1e7d); + global_pos.lat = est_lat; + global_pos.lon = est_lon; 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; - } - - if (local_pos.z_valid) { - global_pos.relative_alt = -local_pos.z; + global_pos.vel_n = local_pos.vx; + global_pos.vel_e = local_pos.vy; } if (local_pos.z_global) { @@ -860,7 +856,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.vel_d = local_pos.vz; } global_pos.yaw = local_pos.yaw; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 46f8ed827..9bac2958e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1252,12 +1252,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); log_msg.msg_type = LOG_GPOS_MSG; - log_msg.body.log_GPOS.lat = buf.global_pos.lat; - log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.lat = buf.global_pos.lat * 1e7; + log_msg.body.log_GPOS.lon = buf.global_pos.lon * 1e7; log_msg.body.log_GPOS.alt = buf.global_pos.alt; - log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; - log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; - log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 3e2fee84e..08d11abae 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -62,7 +62,7 @@ struct home_position_s //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float altitude; /**< Altitude in meters */ + float alt; /**< Altitude in meters */ }; /** diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 143734e37..ae771ca00 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -54,7 +54,7 @@ /** * Fused global position in WGS84. * - * This struct contains the system's believ about its position. It is not the raw GPS + * This struct contains global position estimation. It is not the raw GPS * measurement (@see vehicle_gps_position). This topic is usually published by the position * estimator, which will take more sources of information into account than just GPS, * e.g. control inputs of the vehicle in a Kalman-filter implementation. @@ -65,14 +65,13 @@ struct vehicle_global_position_s 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 */ - 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 yaw; /**< Compass heading in radians -PI..+PI. */ + double lat; /**< Latitude in degrees */ + double lon; /**< Longitude in degrees */ + float alt; /**< Altitude in meters */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ + float yaw; /**< Yaw in radians -PI..+PI. */ }; /** -- cgit v1.2.3 From dfedbcb855e53da5fe2ce4d37d9f4fba54626d62 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Fri, 24 Jan 2014 23:56:54 +0000 Subject: added check for MAIN_STATE_OFFBOARD @#1152 so that we dont get thrown out of this state --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc8278bb7..bec6884e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1149,7 +1149,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + if (status.main_state != MAIN_STATE_AUTO && armed.armed && status.main_state != MAIN_STATE_OFFBOARD) { transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); if (res == TRANSITION_CHANGED) { -- cgit v1.2.3 From ebc7cb03b726ebfb864e770a82b92bb67b6bfd4c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:24:12 +0100 Subject: «flighttermination state» replaced by more general «failsafe state» MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 39 ++-- src/modules/commander/state_machine_helper.cpp | 58 +++-- src/modules/commander/state_machine_helper.h | 4 +- src/modules/fw_att_control/fw_att_control_main.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 238 ++++++++++++--------- src/modules/uORB/topics/vehicle_control_mode.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 11 +- 7 files changed, 198 insertions(+), 156 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d51bb63ff..7715f73e0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -512,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON); + transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -1112,6 +1112,14 @@ int commander_thread_main(int argc, char *argv[]) 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); } + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); + } + } + /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); @@ -1135,32 +1143,23 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } if (status.main_state != MAIN_STATE_AUTO && armed.armed) { - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to RTL mode"); - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (status.main_state != MAIN_STATE_SEATBELT) { - res = main_state_transition(&status, MAIN_STATE_SEATBELT); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe, switching to SEATBELT mode"); - } + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); } + // TODO add other failsafe modes if position estimate not available } } } - /* Flight termination in manual mode if assisted switch is on easy position //xxx hack! */ - if (armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - transition_result_t flighttermination_res = flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_ON); - if (flighttermination_res == TRANSITION_CHANGED) { + // TODO remove this hack + /* flight termination in manual mode if assisted switch is on easy position */ + if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } - } else { - flighttermination_state_transition(&status, FLIGHTTERMINATION_STATE_OFF); } - /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); @@ -1176,12 +1175,12 @@ int commander_thread_main(int argc, char *argv[]) /* 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 flighttermination_state_changed = check_flighttermination_state_changed(); + bool failsafe_state_changed = check_failsafe_state_changed(); hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d", status.arming_state, status.main_state); + if (arming_state_changed || main_state_changed || failsafe_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); status_changed = true; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 731e0e3ff..90ca2a6d2 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -63,7 +63,7 @@ static const int ERROR = -1; static bool arming_state_changed = true; static bool main_state_changed = true; -static bool flighttermination_state_changed = true; +static bool failsafe_state_changed = true; transition_result_t arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, @@ -287,10 +287,10 @@ check_main_state_changed() } bool -check_flighttermination_state_changed() +check_failsafe_state_changed() { - if (flighttermination_state_changed) { - flighttermination_state_changed = false; + if (failsafe_state_changed) { + failsafe_state_changed = false; return true; } else { @@ -361,41 +361,39 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /** -* Transition from one flightermination state to another +* Transition from one failsafe state to another */ -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state) +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_flighttermination_state == status->flighttermination_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { + /* only check transition if the new state is actually different from the current one */ + if (new_failsafe_state == status->failsafe_state) { + ret = TRANSITION_NOT_CHANGED; - switch (new_flighttermination_state) { - case FLIGHTTERMINATION_STATE_ON: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_ON; - warnx("state machine helper: change to FLIGHTTERMINATION_STATE_ON"); - break; - case FLIGHTTERMINATION_STATE_OFF: - ret = TRANSITION_CHANGED; - status->flighttermination_state = FLIGHTTERMINATION_STATE_OFF; - break; + } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + switch (new_failsafe_state) { + case FAILSAFE_STATE_NORMAL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_RTL: + ret = TRANSITION_CHANGED; + break; + case FAILSAFE_STATE_TERMINATION: + ret = TRANSITION_CHANGED; + break; - default: - break; - } + default: + break; + } - if (ret == TRANSITION_CHANGED) { - flighttermination_state_changed = true; - // TODO - //control_mode->flag_control_flighttermination_enabled = status->flighttermination_state == FLIGHTTERMINATION_STATE_ON; - } + if (ret == TRANSITION_CHANGED) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; } + } - return ret; + return ret; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e569fb4f3..f04879ff9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,11 +67,11 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state bool check_main_state_changed(); -transition_result_t flighttermination_state_transition(struct vehicle_status_s *status, flighttermination_state_t new_flighttermination_state); +transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); bool check_navigation_state_changed(); -bool check_flighttermination_state_changed(); +bool check_failsafe_state_changed(); void set_navigation_state_changed(); diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index dc2196de6..17b1028f9 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() } /* Simple handling of failsafe: deploy parachute if failsafe is on */ - if (_vcontrol_mode.flag_control_flighttermination_enabled) { + if (_vcontrol_mode.flag_control_termination_enabled) { _actuators_airframe.control[1] = 1.0f; // warnx("_actuators_airframe.control[1] = 1.0f;"); } else { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index cfcc886b6..89a62e166 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -643,87 +643,101 @@ Navigator::task_main() vehicle_status_update(); pub_control_mode = true; - /* Evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.main_state == MAIN_STATE_AUTO && - (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR)) { - bool stick_mode = false; - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - stick_mode = true; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + /* evaluate state machine from commander and set the navigator mode accordingly */ + if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { + if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (_vstatus.main_state == MAIN_STATE_AUTO) { + bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + stick_mode = true; } else { - dispatch(EVENT_LOITER_REQUESTED); + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + stick_mode = true; + } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } } - stick_mode = true; } - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } - } - } - - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + break; + + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); } else { - dispatch(EVENT_LOITER_REQUESTED); + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - break; - - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; } - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + /* not in AUTO mode */ + dispatch(EVENT_NONE_REQUESTED); } + + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { + /* RTL on failsafe */ + dispatch(EVENT_RTL_REQUESTED); + + } else { + /* shouldn't act */ + dispatch(EVENT_NONE_REQUESTED); } } else { - /* not in AUTO */ + /* not armed */ dispatch(EVENT_NONE_REQUESTED); } } @@ -1442,40 +1456,74 @@ Navigator::publish_control_mode() _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_flighttermination_enabled = false; + _control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator has control */ + bool navigator_enabled = false; + + switch (_vstatus.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (_vstatus.main_state) { + case MAIN_STATE_MANUAL: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; + case MAIN_STATE_SEATBELT: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = false; + _control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + _control_mode.flag_control_manual_enabled = true; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + _control_mode.flag_control_position_enabled = true; + _control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } break; - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + navigator_enabled = true; + /* disable all controllers on termination */ + _control_mode.flag_control_rates_enabled = false; + _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; _control_mode.flag_control_velocity_enabled = false; + _control_mode.flag_control_altitude_enabled = false; + _control_mode.flag_control_climb_rate_enabled = false; + _control_mode.flag_control_termination_enabled = true; break; - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; + default: break; + } - case MAIN_STATE_AUTO: + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ @@ -1493,10 +1541,6 @@ Navigator::publish_control_mode() _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; } - break; - - default: - break; } _control_mode.timestamp = hrt_absolute_time(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 26dcbd985..f9f8414df 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -92,7 +92,7 @@ struct vehicle_control_mode_s 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 flag_control_flighttermination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1a9dec5f5..a3a862d85 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -82,9 +82,11 @@ typedef enum { } hil_state_t; typedef enum { - FLIGHTTERMINATION_STATE_OFF = 0, - FLIGHTTERMINATION_STATE_ON -} flighttermination_state_t; + FAILSAFE_STATE_NORMAL = 0, + FAILSAFE_STATE_RTL, + FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_MAX +} failsafe_state_t; typedef enum { MODE_SWITCH_MANUAL = 0, @@ -173,6 +175,7 @@ struct vehicle_status_s uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ + failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -223,8 +226,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - - flighttermination_state_t flighttermination_state; }; /** -- cgit v1.2.3 From 92ddf7903b4a540215905f01acd1819eac1f176d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 25 Jan 2014 23:37:26 +0100 Subject: commander: more user-friendly states indication --- src/modules/commander/commander.cpp | 43 ++++++++++++++++++++++++++------ src/modules/navigator/navigator_main.cpp | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 ++- 3 files changed, 40 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7715f73e0..722230eff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -575,6 +575,26 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); + char *main_states_str[MAIN_STATE_MAX]; + main_states_str[0] = "MANUAL"; + main_states_str[1] = "SEATBELT"; + main_states_str[2] = "EASY"; + main_states_str[3] = "AUTO"; + + char *arming_states_str[ARMING_STATE_MAX]; + arming_states_str[0] = "INIT"; + arming_states_str[1] = "STANDBY"; + arming_states_str[2] = "ARMED"; + arming_states_str[3] = "ARMED_ERROR"; + arming_states_str[4] = "STANDBY_ERROR"; + arming_states_str[5] = "REBOOT"; + arming_states_str[6] = "IN_AIR_RESTORE"; + + char *failsafe_states_str[FAILSAFE_STATE_MAX]; + failsafe_states_str[0] = "NORMAL"; + failsafe_states_str[1] = "RTL"; + failsafe_states_str[2] = "TERMINATION"; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1108,8 +1128,8 @@ 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, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { @@ -1127,13 +1147,11 @@ 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); tune_positive(); } 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, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } } else { @@ -1179,9 +1197,20 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime t1 = hrt_absolute_time(); - if (arming_state_changed || main_state_changed || failsafe_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, fs %d", status.arming_state, status.main_state, status.failsafe_state); + /* print new state */ + if (arming_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + } + + if (main_state_changed) { + status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + } + + if (failsafe_state_changed) { status_changed = true; + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 89a62e166..8ecc28b11 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -802,7 +802,7 @@ Navigator::task_main() /* notify user about state changes */ if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s -> %s", nav_states_str[prevState], nav_states_str[myState]); + mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; pub_control_mode = true; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a3a862d85..73102090f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -64,6 +64,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_MAX } main_state_t; typedef enum { @@ -73,7 +74,8 @@ typedef enum { ARMING_STATE_ARMED_ERROR, ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE + ARMING_STATE_IN_AIR_RESTORE, + ARMING_STATE_MAX } arming_state_t; typedef enum { -- cgit v1.2.3 From c7f05539382a48d7ecaad3bfdf71261cde2ee8c7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:50:34 +0100 Subject: cammander: state machine can now deny current state (e.g. when position lock lost during EASY mode), added FAILSAFE_STATE_LAND --- src/modules/commander/commander.cpp | 35 +++++++++- src/modules/commander/state_machine_helper.cpp | 91 ++++++++++++++------------ src/modules/navigator/navigator_main.cpp | 58 +++++++++++++++- src/modules/uORB/topics/vehicle_control_mode.h | 1 + src/modules/uORB/topics/vehicle_status.h | 7 +- 5 files changed, 144 insertions(+), 48 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 722230eff..bca0569d5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[]) /* fill current_status according to mode switches */ check_mode_switches(&sp_man, &status); - /* evaluate the main state machine */ + /* evaluate the main state machine according to mode switches */ res = check_main_state_machine(&status); if (res == TRANSITION_CHANGED) { @@ -1160,12 +1160,41 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; } - if (status.main_state != MAIN_STATE_AUTO && armed.armed) { + + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } + } + + } else if (armed.armed) { + /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + } + } } - // TODO add other failsafe modes if position estimate not available } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 90ca2a6d2..77edea546 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -223,51 +223,50 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_main_state == current_state->main_state) { - ret = TRANSITION_NOT_CHANGED; - - } else { - - switch (new_main_state) { - case MAIN_STATE_MANUAL: + /* transition may be denied even if requested the same state because conditions may be changed */ + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; + + case MAIN_STATE_SEATBELT: + + /* need at minimum altitude estimate */ + if (!current_state->is_rotary_wing || + (current_state->condition_local_altitude_valid || + current_state->condition_global_position_valid)) { ret = TRANSITION_CHANGED; - break; - - case MAIN_STATE_SEATBELT: - - /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { - ret = TRANSITION_CHANGED; - } + } - break; + break; - case MAIN_STATE_EASY: + case MAIN_STATE_EASY: - /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } - - break; + /* need at minimum local position estimate */ + if (current_state->condition_local_position_valid || + current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } - case MAIN_STATE_AUTO: + break; - /* need global position estimate */ - if (current_state->condition_global_position_valid) { - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO: - break; + /* need global position estimate */ + if (current_state->condition_global_position_valid) { + ret = TRANSITION_CHANGED; } - if (ret == TRANSITION_CHANGED) { + break; + } + + if (ret == TRANSITION_CHANGED) { + if (current_state->main_state != new_main_state) { current_state->main_state = new_main_state; main_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; } } @@ -367,17 +366,22 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f { transition_result_t ret = TRANSITION_DENIED; - /* only check transition if the new state is actually different from the current one */ - if (new_failsafe_state == status->failsafe_state) { - ret = TRANSITION_NOT_CHANGED; + /* transition may be denied even if requested the same state because conditions may be changed */ + if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { + /* transitions from TERMINATION to other states not allowed */ + if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { + ret = TRANSITION_NOT_CHANGED; + } - } else if (status->failsafe_state != FAILSAFE_STATE_TERMINATION) { + } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - ret = TRANSITION_CHANGED; + if (status->condition_global_position_valid) { + ret = TRANSITION_CHANGED; + } break; case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; @@ -388,8 +392,13 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } if (ret == TRANSITION_CHANGED) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; + if (status->failsafe_state != new_failsafe_state) { + status->failsafe_state = new_failsafe_state; + failsafe_state_changed = true; + + } else { + ret = TRANSITION_NOT_CHANGED; + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d72ed7058..388fefd02 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -216,6 +216,7 @@ private: EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, + EVENT_LAND_REQUESTED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, MAX_EVENT @@ -292,7 +293,7 @@ private: void start_loiter(); void start_mission(); void start_rtl(); - void finish_rtl(); + void start_land(); /** * Guards offboard mission @@ -733,6 +734,12 @@ Navigator::task_main() dispatch(EVENT_RTL_REQUESTED); } + } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { + /* LAND on failsafe */ + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); + } + } else { /* shouldn't act */ dispatch(EVENT_NONE_REQUESTED); @@ -892,6 +899,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, @@ -902,6 +910,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, @@ -912,6 +921,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, @@ -922,6 +932,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, @@ -932,9 +943,21 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, + { + /* STATE_LAND */ + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, + /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + }, }; void @@ -1142,6 +1165,27 @@ Navigator::start_rtl() set_rtl_item(); } +void +Navigator::start_land() +{ + _do_takeoff = false; + _reset_loiter_pos = true; + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.next.valid = false; + + _pos_sp_triplet.current.valid = true; + _pos_sp_triplet.current.type = SETPOINT_TYPE_LAND; + _pos_sp_triplet.current.lat = _global_pos.lat; + _pos_sp_triplet.current.lon = _global_pos.lon; + _pos_sp_triplet.current.alt = _global_pos.alt; + _pos_sp_triplet.current.loiter_direction = 1; + _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; + _pos_sp_triplet.current.yaw = NAN; + + mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); +} + void Navigator::set_rtl_item() { @@ -1508,9 +1552,21 @@ Navigator::publish_control_mode() navigator_enabled = true; break; + case FAILSAFE_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + case FAILSAFE_STATE_TERMINATION: navigator_enabled = true; /* disable all controllers on termination */ + _control_mode.flag_control_manual_enabled = false; _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; _control_mode.flag_control_position_enabled = false; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index f9f8414df..5aecac898 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -67,6 +67,7 @@ typedef enum { NAV_STATE_LOITER, NAV_STATE_MISSION, NAV_STATE_RTL, + NAV_STATE_LAND, NAV_STATE_MAX } nav_state_t; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 73102090f..a5988d3ba 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -84,9 +84,10 @@ typedef enum { } hil_state_t; typedef enum { - FAILSAFE_STATE_NORMAL = 0, - FAILSAFE_STATE_RTL, - FAILSAFE_STATE_TERMINATION, + FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ + FAILSAFE_STATE_RTL, /**< Return To Launch */ + FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX } failsafe_state_t; -- cgit v1.2.3 From 7d2efe9367787cdfc4590f335f600f3b79b2cbc7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:52:33 +0100 Subject: commander, navigator: minor cleanup (refactoring), code style fixed --- src/modules/commander/commander.cpp | 19 +- src/modules/commander/state_machine_helper.cpp | 24 +- src/modules/navigator/navigator_main.cpp | 360 +++++++++++++++---------- 3 files changed, 244 insertions(+), 159 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bca0569d5..15f229bf0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -371,6 +371,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); } } + if (hil_ret == OK) ret = true; @@ -405,6 +406,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; } } + if (arming_res == TRANSITION_CHANGED) ret = true; @@ -447,6 +449,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } } + if (main_res == TRANSITION_CHANGED) ret = true; @@ -486,8 +489,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_OVERRIDE_GOTO: { - // TODO listen vehicle_command topic directly from navigator (?) + // TODO listen vehicle_command topic directly from navigator (?) unsigned int mav_goto = cmd->param1; + if (mav_goto == 0) { // MAV_GOTO_DO_HOLD status->set_nav_state = NAV_STATE_LOITER; status->set_nav_state_timestamp = hrt_absolute_time(); @@ -508,7 +512,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO @@ -900,6 +904,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); + /* only consider battery voltage if system has been running 2s and battery voltage is valid */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; @@ -1135,6 +1140,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); } @@ -1164,14 +1170,18 @@ int commander_thread_main(int argc, char *argv[]) if (status.main_state == MAIN_STATE_AUTO) { /* check if AUTO mode still allowed */ transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (res == TRANSITION_DENIED) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { /* LAND mode denied, switch to failsafe state TERMINATION */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } @@ -1181,15 +1191,20 @@ int commander_thread_main(int argc, char *argv[]) } else if (armed.armed) { /* failsafe for manual modes */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + } else if (res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate), try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else if (res == TRANSITION_DENIED) { res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 77edea546..1be0a16b8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool failsafe_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_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -85,6 +85,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } @@ -219,7 +220,7 @@ check_arming_state_changed() } transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) +main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; @@ -232,9 +233,9 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ - if (!current_state->is_rotary_wing || - (current_state->condition_local_altitude_valid || - current_state->condition_global_position_valid)) { + if (!status->is_rotary_wing || + (status->condition_local_altitude_valid || + status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } @@ -243,8 +244,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: /* need at minimum local position estimate */ - if (current_state->condition_local_position_valid || - current_state->condition_global_position_valid) { + if (status->condition_local_position_valid || + status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -253,7 +254,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: /* need global position estimate */ - if (current_state->condition_global_position_valid) { + if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -261,8 +262,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m } if (ret == TRANSITION_CHANGED) { - if (current_state->main_state != new_main_state) { - current_state->main_state = new_main_state; + if (status->main_state != new_main_state) { + status->main_state = new_main_state; main_state_changed = true; } else { @@ -378,11 +379,14 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_NORMAL: ret = TRANSITION_CHANGED; break; + case FAILSAFE_STATE_RTL: if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case FAILSAFE_STATE_TERMINATION: ret = TRANSITION_CHANGED; break; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 388fefd02..9b5cfa6b4 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -166,12 +166,12 @@ private: bool _mission_item_valid; /**< current mission item valid */ perf_counter_t _loop_perf; /**< loop performance counter */ - + Geofence _geofence; bool _geofence_violation_warning_sent; bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ + bool _inside_fence; /**< vehicle is inside fence */ struct navigation_capabilities_s _nav_caps; @@ -358,7 +358,7 @@ static const int ERROR = -1; Navigator *g_navigator; } -Navigator::Navigator() : +Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), @@ -490,16 +490,20 @@ void Navigator::offboard_mission_update(bool isrotaryWing) { struct mission_s offboard_mission; + if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) { /* Check mission feasibility, for now do not handle the return value, * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; + if (offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); @@ -516,6 +520,7 @@ void Navigator::onboard_mission_update() { struct mission_s onboard_mission; + if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_current_onboard_mission_index(onboard_mission.current_index); @@ -558,11 +563,13 @@ Navigator::task_main() * else clear geofence data in datamanager */ struct stat buffer; - if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + + if (stat(GEOFENCE_FILENAME, &buffer) == 0) { warnx("Try to load geofence.txt"); _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { - if (_geofence.clearDm() > 0 ) + if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else warnx("Could not clear geofence"); @@ -578,7 +585,7 @@ Navigator::task_main() _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); - + /* copy all topics first time */ vehicle_status_update(); parameters_update(); @@ -649,6 +656,7 @@ Navigator::task_main() if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { if (_vstatus.main_state == MAIN_STATE_AUTO) { bool stick_mode = false; + if (!_vstatus.rc_signal_lost) { /* RC signal available, use control switches to set mode */ /* RETURN switch, overrides MISSION switch */ @@ -656,21 +664,27 @@ Navigator::task_main() if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + stick_mode = true; + } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { dispatch(EVENT_LOITER_REQUESTED); stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { /* switch to mission only if available */ if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + stick_mode = true; } + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ dispatch(EVENT_LOITER_REQUESTED); @@ -696,15 +710,18 @@ Navigator::task_main() case NAV_STATE_MISSION: if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } + break; case NAV_STATE_RTL: if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { dispatch(EVENT_RTL_REQUESTED); } + break; default: @@ -717,12 +734,14 @@ Navigator::task_main() if (myState == NAV_STATE_NONE) { if (_mission.current_mission_available()) { dispatch(EVENT_MISSION_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } } } + } else { /* not in AUTO mode */ dispatch(EVENT_NONE_REQUESTED); @@ -765,7 +784,7 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + // XXX check if mission really changed dispatch(EVENT_MISSION_CHANGED); } @@ -786,6 +805,7 @@ Navigator::task_main() /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); + /* only check if waypoint has been reached in MISSION and RTL modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (check_mission_item_reached()) { @@ -794,15 +814,15 @@ Navigator::task_main() } /* Check geofence violation */ - if(!_geofence.inside(&_global_pos)) { + if (!_geofence.inside(&_global_pos)) { //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ - if (!_geofence_violation_warning_sent) - { + if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } + } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; @@ -852,48 +872,55 @@ Navigator::start() } void -Navigator::status() +Navigator::status() { warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + if (_global_pos.valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); } + if (_fence_valid) { warnx("Geofence is valid"); // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); + } else { warnx("Geofence not set"); } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - default: - warnx("State: Unknown"); - break; + case NAV_STATE_NONE: + warnx("State: None"); + break; + + case NAV_STATE_LOITER: + warnx("State: Loiter"); + break; + + case NAV_STATE_MISSION: + warnx("State: Mission"); + break; + + case NAV_STATE_RTL: + warnx("State: RTL"); + break; + + default: + warnx("State: Unknown"); + break; } } StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* STATE_NONE */ + { + /* STATE_NONE */ /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -903,8 +930,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, }, - { - /* STATE_READY */ + { + /* STATE_READY */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, @@ -915,7 +942,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, }, { - /* STATE_LOITER */ + /* STATE_LOITER */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, @@ -925,8 +952,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, }, - { - /* STATE_MISSION */ + { + /* STATE_MISSION */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -936,8 +963,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, }, - { - /* STATE_RTL */ + { + /* STATE_RTL */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -948,7 +975,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state }, { - /* STATE_LAND */ + /* STATE_LAND */ /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, @@ -1013,6 +1040,7 @@ Navigator::start_loiter() if (_global_pos.alt < min_alt_amsl) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); + } else { _pos_sp_triplet.current.alt = _global_pos.alt; mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude"); @@ -1065,22 +1093,22 @@ Navigator::set_mission_item() position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { + _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { /* don't reset RTL state on RTL or LOITER items */ _rtl_state = RTL_STATE_NONE; } if (_vstatus.is_rotary_wing) { if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { /* do special TAKEOFF handling for VTOL */ _need_takeoff = false; @@ -1108,6 +1136,7 @@ Navigator::set_mission_item() _pos_sp_triplet.current.yaw = NAN; _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; } + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { /* will need takeoff after landing */ _need_takeoff = true; @@ -1116,13 +1145,16 @@ Navigator::set_mission_item() if (_do_takeoff) { mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm AMSL", _pos_sp_triplet.current.alt); + } else { if (onboard) { mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); } } + } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ _mission_item_valid = false; @@ -1136,6 +1168,7 @@ Navigator::set_mission_item() if (ret == OK) { position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + } else { /* this will fail for the last WP */ _pos_sp_triplet.next.valid = false; @@ -1149,17 +1182,21 @@ void Navigator::start_rtl() { _do_takeoff = false; + if (_rtl_state == RTL_STATE_NONE) { if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { _rtl_state = RTL_STATE_CLIMB; + } else { _rtl_state = RTL_STATE_RETURN; + if (_reset_loiter_pos) { _mission_item.altitude_is_relative = false; _mission_item.altitude = _global_pos.alt; } } } + _reset_loiter_pos = true; mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); @@ -1191,118 +1228,123 @@ Navigator::set_rtl_item() { switch (_rtl_state) { case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - } + float climb_alt = _home_pos.alt + _parameters.rtl_alt; - _mission_item_valid = true; + if (_vstatus.condition_landed) { + climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + } - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _mission_item_valid = true; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + _mission_item.lat = _global_pos.lat; + _mission_item.lon = _global_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - _pos_sp_triplet.next.valid = false; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); - break; - } - case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; + _pos_sp_triplet.next.valid = false; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // don't change altitude - _mission_item.yaw = NAN; // TODO set heading to home - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); + break; + } - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + case RTL_STATE_RETURN: { + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _pos_sp_triplet.next.valid = false; + _mission_item_valid = true; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); - break; - } - case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + // don't change altitude + _mission_item.yaw = NAN; // TODO set heading to home + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; - _mission_item_valid = true; + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + _pos_sp_triplet.next.valid = false; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: return"); + break; + } - _pos_sp_triplet.next.valid = false; + case RTL_STATE_DESCEND: { + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt + _parameters.land_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); - break; - } - case RTL_STATE_LAND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + _pos_sp_triplet.next.valid = false; - _mission_item_valid = true; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); + break; + } - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; + case RTL_STATE_LAND: { + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + + _mission_item_valid = true; + + _mission_item.lat = _home_pos.lat; + _mission_item.lon = _home_pos.lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _home_pos.alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _parameters.loiter_radius; + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _parameters.acceptance_radius; + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + _pos_sp_triplet.next.valid = false; - _pos_sp_triplet.next.valid = false; + mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); + break; + } - mavlink_log_info(_mavlink_fd, "[navigator] RTL: land"); - break; - } default: { - mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } + mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state); + start_loiter(); + break; + } } publish_position_setpoint_triplet(); @@ -1312,28 +1354,35 @@ void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { sp->valid = true; + if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { /* set home position for RTL item */ sp->lat = _home_pos.lat; sp->lon = _home_pos.lon; sp->alt = _home_pos.alt + _parameters.rtl_alt; + } else { sp->lat = item->lat; sp->lon = item->lon; sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; } + sp->yaw = item->yaw; sp->loiter_radius = item->loiter_radius; sp->loiter_direction = item->loiter_direction; sp->pitch_min = item->pitch_min; + if (item->nav_cmd == NAV_CMD_TAKEOFF) { sp->type = SETPOINT_TYPE_TAKEOFF; + } else if (item->nav_cmd == NAV_CMD_LAND) { sp->type = SETPOINT_TYPE_LAND; + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { sp->type = SETPOINT_TYPE_LOITER; + } else { sp->type = SETPOINT_TYPE_NORMAL; } @@ -1350,6 +1399,7 @@ Navigator::check_mission_item_reached() if (_mission_item.nav_cmd == NAV_CMD_LAND) { if (_vstatus.is_rotary_wing) { return _vstatus.condition_landed; + } else { /* For fw there is currently no landing detector: * make sure control is not stopped when overshooting the landing waypoint */ @@ -1364,7 +1414,7 @@ Navigator::check_mission_item_reached() _mission_item.loiter_radius > 0.01f) { return false; - } + } uint64_t now = hrt_absolute_time(); @@ -1384,18 +1434,20 @@ Navigator::check_mission_item_reached() /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float wp_alt_amsl = _mission_item.altitude; + if (_mission_item.altitude_is_relative) wp_alt_amsl += _home_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); + (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + &dist_xy, &dist_z); if (_do_takeoff) { if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { /* require only altitude for takeoff */ _waypoint_position_reached = true; } + } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1407,9 +1459,11 @@ Navigator::check_mission_item_reached() if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ _waypoint_yaw_reached = true; } + } else { _waypoint_yaw_reached = true; } @@ -1419,20 +1473,22 @@ Navigator::check_mission_item_reached() if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; + if (_mission_item.time_inside > 0.01f) { mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside); } } - + /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { _time_first_inside_orbit = 0; _waypoint_yaw_reached = false; _waypoint_position_reached = false; return true; } } + return false; } @@ -1445,6 +1501,7 @@ Navigator::on_mission_item_reached() /* takeoff completed */ _do_takeoff = false; mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { /* advance by one mission item */ _mission.move_to_next(); @@ -1452,23 +1509,28 @@ Navigator::on_mission_item_reached() if (_mission.current_mission_available()) { set_mission_item(); + } else { /* if no more mission items available then finish mission */ /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); + if (_vstatus.condition_landed) { dispatch(EVENT_READY_REQUESTED); + } else { dispatch(EVENT_LOITER_REQUESTED); } } + } else { /* RTL finished */ if (_rtl_state == RTL_STATE_LAND) { /* landed at home position */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed, landed"); dispatch(EVENT_READY_REQUESTED); + } else { /* next RTL step */ _rtl_state = (RTLState)(_rtl_state + 1); @@ -1546,6 +1608,7 @@ Navigator::publish_control_mode() default: break; } + break; case FAILSAFE_STATE_RTL: @@ -1583,6 +1646,7 @@ Navigator::publish_control_mode() /* navigator has control, set control mode flags according to nav state*/ if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; + if (myState == NAV_STATE_READY) { /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; @@ -1591,6 +1655,7 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + } else { _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; @@ -1669,8 +1734,9 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { - navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); -- cgit v1.2.3 From b7c69262a7e4d51fb7806ab40a4dbb2b0ea4f75b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 11:58:50 +0100 Subject: state_machine_helper: added missed transition to FAILSAFE_STATE_LAND, transition conditions fixed --- src/modules/commander/state_machine_helper.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1be0a16b8..c7256583a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -377,17 +377,28 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f } else { switch (new_failsafe_state) { case FAILSAFE_STATE_NORMAL: + /* always allowed (except from TERMINATION state) */ ret = TRANSITION_CHANGED; break; case FAILSAFE_STATE_RTL: - if (status->condition_global_position_valid) { + /* global position and home position required for RTL */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } + + break; + + case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ + if (status->condition_local_altitude_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } break; case FAILSAFE_STATE_TERMINATION: + /* always allowed */ ret = TRANSITION_CHANGED; break; -- cgit v1.2.3 From c841929e3f617e67adec6606b3ec6517aa455834 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 14:12:27 +0100 Subject: commander: «home position set» condition fixed, failsafe fixes, navigator: state indication bugfix, control_mode fixes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/modules/commander/commander.cpp | 226 +++++++++++++++---------------- src/modules/navigator/navigator_main.cpp | 34 +++-- 2 files changed, 133 insertions(+), 127 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 15f229bf0..0e9172800 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -565,7 +565,6 @@ 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; @@ -597,7 +596,8 @@ int commander_thread_main(int argc, char *argv[]) char *failsafe_states_str[FAILSAFE_STATE_MAX]; failsafe_states_str[0] = "NORMAL"; failsafe_states_str[1] = "RTL"; - failsafe_states_str[2] = "TERMINATION"; + failsafe_states_str[2] = "LAND"; + failsafe_states_str[3] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -628,6 +628,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = 0; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; + status.failsafe_state = FAILSAFE_STATE_NORMAL; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -1027,13 +1028,12 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && + (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed && global_position.valid) { - /* copy position data to uORB home message, store it locally as well */ - + /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; @@ -1050,164 +1050,160 @@ int commander_thread_main(int argc, char *argv[]) } /* mark home position as set */ - home_position_set = true; + status.condition_home_position_valid = true; tune_positive(); } } - /* ignore RC signals if in offboard control mode */ - if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC input check */ - 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; - mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time"); - status_changed = true; + /* start RC input check */ + if (sp_man.timestamp != 0 && 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; + 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, "#audio: RC signal regained"); - status_changed = true; - } + } else { + if (status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + status_changed = true; } + } - status.rc_signal_lost = false; - - transition_result_t res; // store all transitions results here + status.rc_signal_lost = false; - /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + transition_result_t res; // store all transitions results here - /* 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* arm/disarm by RC */ + res = TRANSITION_NOT_CHANGED; - 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; + /* 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.condition_landed) && + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - } else { - stick_off_counter++; - } + 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++; } - /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ - if (status.arming_state == ARMING_STATE_STANDBY && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - 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 { + stick_off_counter = 0; + } - } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - } + /* check if left stick is in lower right position and we're in MANUAL mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + if (safety.safety_switch_available && !safety.safety_off) { + print_reject_arm("NOT ARMING: Press safety switch first."); - stick_on_counter = 0; + } else if (status.main_state != MAIN_STATE_MANUAL) { + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - stick_on_counter++; + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } - } else { stick_on_counter = 0; + + } else { + stick_on_counter++; } - if (res == TRANSITION_CHANGED) { - if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + } else { + stick_on_counter = 0; + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); - } + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + } - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); - } + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* recover from failsafe */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); } + } - /* fill current_status according to mode switches */ - check_mode_switches(&sp_man, &status); + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); - /* evaluate the main state machine according to mode switches */ - res = check_main_state_machine(&status); + /* evaluate the main state machine according to mode switches */ + res = check_main_state_machine(&status); - if (res == TRANSITION_CHANGED) { - tune_positive(); + if (res == TRANSITION_CHANGED) { + tune_positive(); - } else if (res == TRANSITION_DENIED) { - /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); - } + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + } - } else { - if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); - status.rc_signal_lost = true; - status_changed = true; - } + } else { + if (!status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + status.rc_signal_lost = true; + status_changed = true; + } - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); - } else if (res == TRANSITION_DENIED) { - /* LAND mode denied, switch to failsafe state TERMINATION */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else if (res == TRANSITION_DENIED) { + /* LAND mode denied, switch to failsafe state TERMINATION */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } } + } - } else if (armed.armed) { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + } else if (armed.armed) { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + + } else if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); + mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); } else if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); - - } else if (res == TRANSITION_DENIED) { - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); - } + mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9b5cfa6b4..7c37237ff 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -411,11 +411,13 @@ Navigator::Navigator() : memset(&_mission_result, 0, sizeof(struct mission_result_s)); memset(&_mission_item, 0, sizeof(struct mission_item_s)); + memset(&nav_states_str, 0, sizeof(nav_states_str)); nav_states_str[0] = "NONE"; nav_states_str[1] = "READY"; nav_states_str[2] = "LOITER"; nav_states_str[3] = "MISSION"; nav_states_str[4] = "RTL"; + nav_states_str[5] = "LAND"; /* Initialize state machine */ myState = NAV_STATE_NONE; @@ -750,6 +752,7 @@ Navigator::task_main() } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { /* RTL on failsafe */ if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); } @@ -981,7 +984,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, }, @@ -1220,7 +1223,7 @@ Navigator::start_land() _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.yaw = NAN; - mavlink_log_info(_mavlink_fd, "[navigator] LAND started"); + mavlink_log_info(_mavlink_fd, "[navigator] land started"); } void @@ -1616,14 +1619,7 @@ Navigator::publish_control_mode() break; case FAILSAFE_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; + navigator_enabled = true; break; case FAILSAFE_STATE_TERMINATION: @@ -1647,7 +1643,8 @@ Navigator::publish_control_mode() if (navigator_enabled) { _control_mode.flag_control_manual_enabled = false; - if (myState == NAV_STATE_READY) { + switch (myState) { + case NAV_STATE_READY: /* disable all controllers, armed but idle */ _control_mode.flag_control_rates_enabled = false; _control_mode.flag_control_attitude_enabled = false; @@ -1655,14 +1652,27 @@ Navigator::publish_control_mode() _control_mode.flag_control_velocity_enabled = false; _control_mode.flag_control_altitude_enabled = false; _control_mode.flag_control_climb_rate_enabled = false; + break; - } else { + case NAV_STATE_LAND: + /* land with or without position control */ + _control_mode.flag_control_manual_enabled = false; + _control_mode.flag_control_rates_enabled = true; + _control_mode.flag_control_attitude_enabled = true; + _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; + _control_mode.flag_control_altitude_enabled = true; + _control_mode.flag_control_climb_rate_enabled = true; + break; + + default: _control_mode.flag_control_rates_enabled = true; _control_mode.flag_control_attitude_enabled = true; _control_mode.flag_control_position_enabled = true; _control_mode.flag_control_velocity_enabled = true; _control_mode.flag_control_altitude_enabled = true; _control_mode.flag_control_climb_rate_enabled = true; + break; } } -- cgit v1.2.3 From 9e9105048ac70b7abaa40ef8ce3f6f75910ada0a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 26 Jan 2014 15:46:14 +0100 Subject: commander, navigator: failsafe fixes, mavlink messages cleanup --- src/modules/commander/commander.cpp | 69 ++++++++++++++------------------ src/modules/navigator/navigator_main.cpp | 8 +--- 2 files changed, 31 insertions(+), 46 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0e9172800..4c29d774a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1080,9 +1080,9 @@ int commander_thread_main(int argc, char *argv[]) /* 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) && + 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 */ @@ -1100,7 +1100,7 @@ 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 && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1138,10 +1138,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: recover from failsafe"); - } } /* fill current_status according to mode switches */ @@ -1165,48 +1161,41 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - } else if (res == TRANSITION_DENIED) { - /* LAND mode denied, switch to failsafe state TERMINATION */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } - } - } else if (armed.armed) { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: RTL"); - - } else if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: LAND"); + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - } else if (res == TRANSITION_DENIED) { - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + if (res == TRANSITION_DENIED) { + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#audio: failsafe: TERMINATION"); + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } + + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + } } } diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 7c37237ff..4be9f055b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1072,7 +1072,6 @@ Navigator::start_mission() { _need_takeoff = true; - mavlink_log_info(_mavlink_fd, "[navigator] mission started"); set_mission_item(); } @@ -1201,7 +1200,6 @@ Navigator::start_rtl() } _reset_loiter_pos = true; - mavlink_log_info(_mavlink_fd, "[navigator] RTL started"); set_rtl_item(); } @@ -1222,8 +1220,6 @@ Navigator::start_land() _pos_sp_triplet.current.loiter_direction = 1; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; _pos_sp_triplet.current.yaw = NAN; - - mavlink_log_info(_mavlink_fd, "[navigator] land started"); } void @@ -1259,7 +1255,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm", climb_alt - _home_pos.alt); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt); break; } @@ -1312,7 +1308,7 @@ Navigator::set_rtl_item() _pos_sp_triplet.next.valid = false; - mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm AMSL", _mission_item.altitude); + mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt); break; } -- cgit v1.2.3 From 20108ed95d5bbae64bfcb95de5404fa97d9d0ac1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 13:18:54 +0100 Subject: commander: minor refactoring current_status -> status --- src/modules/commander/commander.cpp | 58 ++++++++++++++++++------------------- 1 file changed, 29 insertions(+), 29 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 4c29d774a..f579fb52a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -199,9 +199,9 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua 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); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status); void print_reject_mode(const char *msg); @@ -1140,11 +1140,11 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill current_status according to mode switches */ + /* fill status according to mode switches */ check_mode_switches(&sp_man, &status); /* evaluate the main state machine according to mode switches */ - res = check_main_state_machine(&status); + res = set_main_state_rc(&status); if (res == TRANSITION_CHANGED) { tune_positive(); @@ -1418,72 +1418,72 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) { /* main mode switch */ if (!isfinite(sp_man->mode_switch)) { warnx("mode sw not finite"); - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_AUTO; + status->mode_switch = MODE_SWITCH_AUTO; } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - current_status->mode_switch = MODE_SWITCH_MANUAL; + status->mode_switch = MODE_SWITCH_MANUAL; } else { - current_status->mode_switch = MODE_SWITCH_ASSISTED; + status->mode_switch = MODE_SWITCH_ASSISTED; } /* return switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + status->return_switch = RETURN_SWITCH_NONE; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + status->return_switch = RETURN_SWITCH_RETURN; } else { - current_status->return_switch = RETURN_SWITCH_NORMAL; + status->return_switch = RETURN_SWITCH_NORMAL; } /* assisted switch */ if (!isfinite(sp_man->assisted_switch)) { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - current_status->assisted_switch = ASSISTED_SWITCH_EASY; + status->assisted_switch = ASSISTED_SWITCH_EASY; } else { - current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } /* mission switch */ if (!isfinite(sp_man->mission_switch)) { - current_status->mission_switch = MISSION_SWITCH_NONE; + status->mission_switch = MISSION_SWITCH_NONE; } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - current_status->mission_switch = MISSION_SWITCH_LOITER; + status->mission_switch = MISSION_SWITCH_LOITER; } else { - current_status->mission_switch = MISSION_SWITCH_MISSION; + status->mission_switch = MISSION_SWITCH_MISSION; } } transition_result_t -check_main_state_machine(struct vehicle_status_s *current_status) +set_main_state_rc(struct vehicle_status_s *status) { /* evaluate the main state machine */ transition_result_t res = TRANSITION_DENIED; - switch (current_status->mode_switch) { + switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL); + res = main_state_transition(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); + if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state @@ -1492,34 +1492,34 @@ check_main_state_machine(struct vehicle_status_s *current_status) print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(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 + if (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); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO); + res = main_state_transition(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) print_reject_mode("AUTO"); - res = main_state_transition(current_status, MAIN_STATE_SEATBELT); + res = main_state_transition(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); + res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; -- cgit v1.2.3 From d1508a7813ad09a173fe314608c25dc8c3cd7a1f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 27 Jan 2014 20:49:17 +0100 Subject: vehicle_control_mode publication moved to commander, WIP --- src/modules/commander/commander.cpp | 136 ++++++++-- src/modules/commander/state_machine_helper.cpp | 4 + src/modules/mavlink/mavlink.c | 4 + src/modules/navigator/navigator_main.cpp | 342 ++++++++----------------- src/modules/navigator/navigator_state.h | 21 ++ src/modules/sdlog2/sdlog2.c | 4 +- src/modules/uORB/topics/vehicle_control_mode.h | 19 +- src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 255 insertions(+), 277 deletions(-) create mode 100644 src/modules/navigator/navigator_state.h (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f579fb52a..60fb4f486 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -203,6 +203,8 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t set_main_state_rc(struct vehicle_status_s *status); +void set_control_mode(); + void print_reject_mode(const char *msg); void print_reject_arm(const char *msg); @@ -555,10 +557,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } static struct vehicle_status_s status; - -/* armed topic */ +static struct vehicle_control_mode_s control_mode; static struct actuator_armed_s armed; - static struct safety_s safety; int commander_thread_main(int argc, char *argv[]) @@ -613,16 +613,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - /* Main state machine */ - /* make sure we are in preflight state */ + /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value - - /* armed topic */ - orb_advert_t armed_pub; - /* Initialize armed with all false */ - memset(&armed, 0, sizeof(armed)); - status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; @@ -645,14 +638,20 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; - /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), &status); - /* publish current state machine */ - - /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); + + /* publish initial state */ + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + + /* armed topic */ + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); + + /* vehicle control mode topic */ + memset(&control_mode, 0, sizeof(control_mode)); + orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -1244,8 +1243,13 @@ int commander_thread_main(int argc, char *argv[]) /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + set_control_mode(); + control_mode.timestamp = t1; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1472,7 +1476,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta transition_result_t set_main_state_rc(struct vehicle_status_s *status) { - /* evaluate the main state machine */ + /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; switch (status->mode_switch) { @@ -1530,6 +1534,102 @@ set_main_state_rc(struct vehicle_status_s *status) return res; } +void +set_control_mode() +{ + /* set vehicle_control_mode according to main state and failsafe state */ + control_mode.flag_armed = armed.armed; + control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; + control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + + control_mode.flag_control_termination_enabled = false; + + /* set this flag when navigator should act */ + bool navigator_enabled = false; + + switch (status.failsafe_state) { + case FAILSAFE_STATE_NORMAL: + switch (status.main_state) { + case MAIN_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_SEATBELT: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + case MAIN_STATE_EASY: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + + case MAIN_STATE_AUTO: + navigator_enabled = true; + + default: + break; + } + + break; + + case FAILSAFE_STATE_RTL: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_LAND: + navigator_enabled = true; + break; + + case FAILSAFE_STATE_TERMINATION: + /* disable all controllers on termination */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_termination_enabled = true; + break; + + default: + break; + } + + /* navigator has control, set control mode flags according to nav state*/ + if (navigator_enabled) { + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + } +} + void print_reject_mode(const char *msg) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c7256583a..43d0e023e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,6 +384,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_RTL: /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->set_nav_state = NAV_STATE_RTL; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } @@ -392,6 +394,8 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f case FAILSAFE_STATE_LAND: /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { + status->set_nav_state = NAV_STATE_LAND; + status->set_nav_state_timestamp = hrt_absolute_time(); ret = TRANSITION_CHANGED; } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4f8091716..5406c35c2 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -220,6 +220,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } 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; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + // TODO get nav state + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + /* if (control_mode.nav_state == NAV_STATE_NONE) { // failsafe, shouldn't happen custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (control_mode.nav_state == NAV_STATE_READY) { @@ -231,6 +234,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u } else if (control_mode.nav_state == NAV_STATE_RTL) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; } + */ } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dd471928e..2117755ee 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -84,6 +84,7 @@ #include #include +#include "navigator_state.h" #include "navigator_mission.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -151,10 +152,10 @@ private: int _offboard_mission_sub; /**< notification of offboard mission updates */ int _onboard_mission_sub; /**< notification of onboard mission updates */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ - orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ struct vehicle_status_s _vstatus; /**< vehicle status */ struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -274,6 +275,10 @@ private: */ void vehicle_status_update(); + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); /** * Shim for calling task_main from task_create. @@ -341,11 +346,6 @@ private: * Publish a new mission item triplet for position controller */ void publish_position_setpoint_triplet(); - - /** - * Publish vehicle_control_mode topic for controllers - */ - void publish_control_mode(); }; namespace navigator @@ -373,6 +373,7 @@ Navigator::Navigator() : _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), + _control_mode_sub(-1), _params_sub(-1), _offboard_mission_sub(-1), _onboard_mission_sub(-1), @@ -381,7 +382,6 @@ Navigator::Navigator() : /* publications */ _pos_sp_triplet_pub(-1), _mission_result_pub(-1), - _control_mode_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), @@ -541,9 +541,19 @@ Navigator::onboard_mission_update() void Navigator::vehicle_status_update() { - /* try to load initial states */ if (orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &_vstatus) != OK) { - _vstatus.arming_state = ARMING_STATE_STANDBY; /* in case the commander is not be running */ + /* in case the commander is not be running */ + _vstatus.arming_state = ARMING_STATE_STANDBY; + } +} + +void +Navigator::vehicle_control_mode_update() +{ + if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) != OK) { + /* in case the commander is not be running */ + _control_mode.flag_control_auto_enabled = false; + _control_mode.flag_armed = false; } } @@ -589,11 +599,13 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); /* copy all topics first time */ vehicle_status_update(); + vehicle_control_mode_update(); parameters_update(); global_position_update(); home_position_update(); @@ -605,12 +617,11 @@ Navigator::task_main() orb_set_interval(_global_pos_sub, 20); unsigned prevState = NAV_STATE_NONE; - bool pub_control_mode = true; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _params_sub; @@ -627,6 +638,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _vstatus_sub; fds[6].events = POLLIN; + fds[7].fd = _control_mode_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -652,127 +665,113 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* vehicle control mode updated */ + if (fds[7].revents & POLLIN) { + vehicle_control_mode_update(); + } + /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - pub_control_mode = true; /* evaluate state machine from commander and set the navigator mode accordingly */ - if (_vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR) { - if (_vstatus.failsafe_state == FAILSAFE_STATE_NORMAL) { - if (_vstatus.main_state == MAIN_STATE_AUTO) { - bool stick_mode = false; + if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { + bool stick_mode = false; + + if (!_vstatus.rc_signal_lost) { + /* RC signal available, use control switches to set mode */ + /* RETURN switch, overrides MISSION switch */ + if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + stick_mode = true; + + } else { + /* MISSION switch */ + if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; - stick_mode = true; + } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { + /* switch to mission only if available */ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - - stick_mode = true; - } - - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); - stick_mode = true; - } + dispatch(EVENT_LOITER_REQUESTED); } + + stick_mode = true; } - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + dispatch(EVENT_LOITER_REQUESTED); + stick_mode = true; + } + } + } - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; + if (!stick_mode) { + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; - case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); + case NAV_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + case NAV_STATE_MISSION: + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - break; + } else { + dispatch(EVENT_LOITER_REQUESTED); + } - case NAV_STATE_RTL: - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { - dispatch(EVENT_RTL_REQUESTED); - } + break; - break; + case NAV_STATE_RTL: + if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + dispatch(EVENT_RTL_REQUESTED); + } - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } + break; - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - } + case NAV_STATE_LAND: + if (myState != NAV_STATE_READY) { + dispatch(EVENT_LAND_REQUESTED); } - } - } else { - /* not in AUTO mode */ - dispatch(EVENT_NONE_REQUESTED); - } + break; - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_RTL) { - /* RTL on failsafe */ - if (myState != NAV_STATE_READY || _rtl_state != RTL_STATE_LAND) { + default: + warnx("ERROR: Requested navigation state not supported"); + break; + } - dispatch(EVENT_RTL_REQUESTED); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); - } else if (_vstatus.failsafe_state == FAILSAFE_STATE_LAND) { - /* LAND on failsafe */ - if (myState != NAV_STATE_READY) { - dispatch(EVENT_LAND_REQUESTED); + } else { + dispatch(EVENT_LOITER_REQUESTED); + } + } } - - } else { - /* shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); } } else { - /* not armed */ + /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } } @@ -840,12 +839,6 @@ Navigator::task_main() if (myState != prevState) { mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]); prevState = myState; - pub_control_mode = true; - } - - /* publish control mode if updated */ - if (pub_control_mode) { - publish_control_mode(); } perf_end(_loop_perf); @@ -1556,139 +1549,6 @@ Navigator::publish_position_setpoint_triplet() } } -void -Navigator::publish_control_mode() -{ - /* update vehicle_control_mode topic*/ - _control_mode.main_state = _vstatus.main_state; - _control_mode.nav_state = static_cast(myState); - _control_mode.flag_armed = _vstatus.arming_state == ARMING_STATE_ARMED || _vstatus.arming_state == ARMING_STATE_ARMED_ERROR; - _control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing; - _control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON; - - _control_mode.flag_control_offboard_enabled = false; - _control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator has control */ - bool navigator_enabled = false; - - switch (_vstatus.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (_vstatus.main_state) { - case MAIN_STATE_MANUAL: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_SEATBELT: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_EASY: - _control_mode.flag_control_manual_enabled = true; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - - default: - break; - } - - break; - - case FAILSAFE_STATE_RTL: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_LAND: - navigator_enabled = true; - break; - - case FAILSAFE_STATE_TERMINATION: - navigator_enabled = true; - /* disable all controllers on termination */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - _control_mode.flag_control_termination_enabled = true; - break; - - default: - break; - } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - _control_mode.flag_control_manual_enabled = false; - - switch (myState) { - case NAV_STATE_READY: - /* disable all controllers, armed but idle */ - _control_mode.flag_control_rates_enabled = false; - _control_mode.flag_control_attitude_enabled = false; - _control_mode.flag_control_position_enabled = false; - _control_mode.flag_control_velocity_enabled = false; - _control_mode.flag_control_altitude_enabled = false; - _control_mode.flag_control_climb_rate_enabled = false; - break; - - case NAV_STATE_LAND: - /* land with or without position control */ - _control_mode.flag_control_manual_enabled = false; - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_velocity_enabled = _vstatus.condition_global_position_valid; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - - default: - _control_mode.flag_control_rates_enabled = true; - _control_mode.flag_control_attitude_enabled = true; - _control_mode.flag_control_position_enabled = true; - _control_mode.flag_control_velocity_enabled = true; - _control_mode.flag_control_altitude_enabled = true; - _control_mode.flag_control_climb_rate_enabled = true; - break; - } - } - - _control_mode.timestamp = hrt_absolute_time(); - - /* lazily publish the mission triplet only once available */ - if (_control_mode_pub > 0) { - /* publish the mission triplet */ - orb_publish(ORB_ID(vehicle_control_mode), _control_mode_pub, &_control_mode); - - } else { - /* advertise and publish */ - _control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &_control_mode); - } -} - void Navigator::add_fence_point(int argc, char *argv[]) { _geofence.addPoint(argc, argv); diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h new file mode 100644 index 000000000..6a1475c9b --- /dev/null +++ b/src/modules/navigator/navigator_state.h @@ -0,0 +1,21 @@ +/* + * navigator_state.h + * + * Created on: 27.01.2014 + * Author: ton + */ + +#ifndef NAVIGATOR_STATE_H_ +#define NAVIGATOR_STATE_H_ + +typedef enum { + NAV_STATE_NONE = 0, + NAV_STATE_READY, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + +#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9bac2958e..1e032d1c8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1067,8 +1067,8 @@ int sdlog2_thread_main(int argc, char *argv[]) /* copy VEHICLE CONTROL MODE control mode here to construct STAT message */ orb_copy(ORB_ID(vehicle_control_mode), subs.control_mode_sub, &buf.control_mode); log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.main_state = (uint8_t) buf.control_mode.main_state; - log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; + log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; + //log_msg.body.log_STAT.navigation_state = (uint8_t) buf.control_mode.nav_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 5aecac898..7cbb37cd8 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,23 +61,10 @@ * Encodes the complete system state and is set by the commander app. */ -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - struct vehicle_control_mode_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ - main_state_t main_state; - nav_state_t nav_state; - bool flag_armed; bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ @@ -86,14 +73,14 @@ struct vehicle_control_mode_s bool flag_system_hil_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ 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 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 */ - bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ + bool flag_control_termination_enabled; /**< true if flighttermination is enabled */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a5988d3ba..1b3639e30 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,6 +54,8 @@ #include #include "../uORB.h" +#include + /** * @addtogroup topics @{ */ -- cgit v1.2.3 From 6a1a29f77ecc9ded341bfbca037c9a6768ed3fb4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 28 Jan 2014 20:40:05 +0100 Subject: global_position topic: added baro_alt, mc_pos_control: SEATBELT mode fixed, use baro/AMSL alt --- src/drivers/frsky_telemetry/frsky_data.c | 2 +- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 2 +- .../attitude_estimator_ekf_main.cpp | 2 +- src/modules/commander/commander.cpp | 4 +- src/modules/mavlink/mavlink_receiver.cpp | 2 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 45 +++++++++++++++++++--- src/modules/navigator/navigator_main.cpp | 5 ++- .../position_estimator_inav_main.c | 23 ++++------- src/modules/sdlog2/sdlog2.c | 2 + src/modules/sdlog2/sdlog2_messages.h | 4 +- src/modules/uORB/topics/vehicle_global_position.h | 12 ++++-- 11 files changed, 69 insertions(+), 34 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index e201ecbb3..cfcf91e3f 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,7 +225,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.valid) { + if (global_pos.global_valid) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 8e88130e1..7f0dd9219 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -314,7 +314,7 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.valid = true; + _pos.global_valid = true; _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); _pos.alt = float(alt); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 66ec20b95..620185fb7 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 60fb4f486..901f91911 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -871,7 +871,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1030,7 +1030,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.valid) { + && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c23488d7..a371a499e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -634,7 +634,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); // global position packet hil_global_pos.timestamp = timestamp; - hil_global_pos.valid = true; + hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4ff13d4df..923a9dab0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -182,6 +182,7 @@ private: bool _reset_lat_lon_sp; bool _reset_alt_sp; + bool _use_global_alt; /**< switch between global (AMSL) and barometric altitudes */ math::Vector<3> _vel; math::Vector<3> _vel_sp; @@ -214,6 +215,11 @@ private: */ void reset_alt_sp(); + /** + * Select between barometric and global (AMSL) altitudes + */ + void select_alt(bool global); + /** * Shim for calling task_main from task_create. */ @@ -263,7 +269,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _alt_sp(0.0f), _reset_lat_lon_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _use_global_alt(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -466,8 +473,23 @@ MulticopterPositionControl::reset_alt_sp() { if (_reset_alt_sp) { _reset_alt_sp = false; - _alt_sp = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", (double)_alt_sp); + _alt_sp = _use_global_alt ? _global_pos.alt : _global_pos.baro_alt; + mavlink_log_info(_mavlink_fd, "[mpc] reset alt (%s) sp: %.2f", _use_global_alt ? "AMSL" : "baro", (double)_alt_sp); + } +} + +void +MulticopterPositionControl::select_alt(bool global) +{ + if (global != _use_global_alt) { + _use_global_alt = global; + if (global) { + /* switch from barometric to global altitude */ + _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { + /* switch from global to barometric altitude */ + _alt_sp += _global_pos.baro_alt - _global_pos.alt; + } } } @@ -565,8 +587,16 @@ MulticopterPositionControl::task_main() sp_move_rate.zero(); + float alt = _global_pos.alt; + /* select control source */ if (_control_mode.flag_control_manual_enabled) { + /* select altitude source and update setpoint */ + select_alt(_global_pos.global_valid); + if (!_use_global_alt) { + alt = _global_pos.baro_alt; + } + /* manual control */ if (_control_mode.flag_control_altitude_enabled) { /* reset alt setpoint to current altitude if needed */ @@ -612,7 +642,7 @@ MulticopterPositionControl::task_main() } if (_control_mode.flag_control_altitude_enabled) { - pos_sp_offs(2) = -(_alt_sp - _global_pos.alt) / _params.sp_offs_max(2); + pos_sp_offs(2) = -(_alt_sp - alt) / _params.sp_offs_max(2); } float pos_sp_offs_norm = pos_sp_offs.length(); @@ -620,7 +650,7 @@ MulticopterPositionControl::task_main() if (pos_sp_offs_norm > 1.0f) { pos_sp_offs /= pos_sp_offs_norm; add_vector_to_global_position(_lat_sp, _lon_sp, pos_sp_offs(0) * _params.sp_offs_max(0), pos_sp_offs(1) * _params.sp_offs_max(1), &_lat_sp, &_lon_sp); - _alt_sp = _global_pos.alt - pos_sp_offs(2) * _params.sp_offs_max(2); + _alt_sp = alt - pos_sp_offs(2) * _params.sp_offs_max(2); } /* fill position setpoint triplet */ @@ -647,6 +677,9 @@ MulticopterPositionControl::task_main() } } else if (_control_mode.flag_control_auto_enabled) { + /* always use AMSL altitude for AUTO */ + select_alt(true); + /* AUTO */ bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -678,7 +711,7 @@ MulticopterPositionControl::task_main() math::Vector<3> pos_err; float err_x, err_y; get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); - pos_err(2) = -(_alt_sp - _global_pos.alt); + pos_err(2) = -(_alt_sp - alt); _vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5a02bf522..170e5df05 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -840,6 +840,7 @@ Navigator::task_main() /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { + _pos_sp_triplet_updated = false; publish_position_setpoint_triplet(); } @@ -882,9 +883,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.valid ? "" : "in"); + warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); - if (_global_pos.valid) { + if (_global_pos.global_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); 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 af04bb0bc..e045ce4cc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -202,8 +202,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool landed = true; hrt_abstime landed_time = 0; - bool flag_armed = false; - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -329,6 +327,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; + global_pos.baro_valid = true; } } } @@ -379,17 +378,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - - /* reset ground level on arm */ - if (armed.armed && !flag_armed) { - flag_armed = armed.armed; - baro_offset -= z_est[0]; - corr_baro = 0.0f; - local_pos.ref_alt -= z_est[0]; - local_pos.ref_timestamp = t; - z_est[0] = 0.0f; - alt_avg = 0.0f; - } } /* sensor combined */ @@ -637,6 +625,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; + dt = fmaxf(fminf(0.02, dt), 0.005); t_prev = t; /* use GPS if it's valid and reference position initialized */ @@ -679,7 +668,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (use_gps_z) { float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; - baro_counter += offs_corr; + corr_baro += offs_corr; } /* accelerometer bias correction */ @@ -835,7 +824,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); /* publish global position */ - global_pos.valid = local_pos.xy_global; + global_pos.global_valid = local_pos.xy_global; if (local_pos.xy_global) { double est_lat, est_lon; @@ -855,6 +844,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.alt = local_pos.ref_alt - local_pos.z; } + if (local_pos.z_valid) { + global_pos.baro_alt = baro_offset - local_pos.z; + } + if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c3ea30cbf..3c218e21f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1244,6 +1244,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; + log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; + log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index baac2ee3e..db87e3a6a 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -204,6 +204,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; + float baro_alt; + uint8_t flags; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -303,7 +305,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ae771ca00..ff9e98e1c 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -61,17 +61,21 @@ */ 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 */ - bool valid; /**< true if position satisfies validity criteria of estimator */ + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool global_valid; /**< true if position satisfies validity criteria of estimator */ + bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ + + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters */ + float alt; /**< Altitude AMSL in meters */ float vel_n; /**< Ground north velocity, m/s */ float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ + + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** -- cgit v1.2.3 From 59a5f37b7feb0cecfdd983e88ce55931f3c1bce1 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 29 Jan 2014 15:39:58 +0000 Subject: Added check for offboard_control_signal_lost when switching to offboard mode with rc switch --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bec6884e2..3687f55fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,8 +1478,8 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta if (!isfinite(sp_man->offboard_switch)) { current_status->offboard_switch = OFFBOARD_SWITCH_NONE; - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT && !status.offboard_control_signal_lost) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; -- cgit v1.2.3 From 01c9092213449c761759bcda11ef9613226be713 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 29 Jan 2014 18:06:03 +0100 Subject: Revert "Added check for offboard_control_signal_lost when switching to offboard mode with rc switch" This reverts commit 59a5f37b7feb0cecfdd983e88ce55931f3c1bce1. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3687f55fb..bec6884e2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1478,8 +1478,8 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta if (!isfinite(sp_man->offboard_switch)) { current_status->offboard_switch = OFFBOARD_SWITCH_NONE; - } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT && !status.offboard_control_signal_lost) { - current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + current_status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; } else { current_status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; -- cgit v1.2.3 From 7f4f9a5f5f6c4921453f3e0c98531bcb4e44d981 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 22:44:05 +0100 Subject: ACRO mode implemented --- src/modules/commander/commander.cpp | 35 ++++++- src/modules/commander/state_machine_helper.cpp | 4 + src/modules/mc_att_control/mc_att_control_main.cpp | 104 +++++++++++++++------ src/modules/mc_att_control/mc_att_control_params.c | 3 + src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++ src/modules/uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/rc_channels.h | 11 ++- src/modules/uORB/topics/vehicle_status.h | 8 ++ 9 files changed, 145 insertions(+), 36 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713..89294fc76 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,9 +619,10 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; - main_states_str[3] = "AUTO"; + main_states_str[1] = "ACRO"; + main_states_str[2] = "SEATBELT"; + main_states_str[3] = "EASY"; + main_states_str[4] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { status->mission_switch = MISSION_SWITCH_MISSION; } + + /* acro switch */ + if (!isfinite(sp_man->acro_switch)) { + status->acro_switch = ACRO_SWITCH_NONE; + + } else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) { + status->acro_switch = ACRO_SWITCH_ACRO; + + } else { + status->acro_switch = ACRO_SWITCH_NORMAL; + } } transition_result_t @@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status) switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (status->acro_switch == ACRO_SWITCH_ACRO) { + res = main_state_transition(status, MAIN_STATE_ACRO); + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1600,6 +1616,17 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case MAIN_STATE_SEATBELT: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e..fd966a068 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -230,6 +230,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; + case MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0accb855..5f862652a 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -161,7 +161,12 @@ private: param_t yaw_rate_i; param_t yaw_rate_d; param_t yaw_ff; + param_t roll_scale_acro; + param_t pitch_scale_acro; + param_t yaw_scale_acro; + param_t rc_scale_roll; + param_t rc_scale_pitch; param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ @@ -171,8 +176,9 @@ private: math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ + math::Vector<3> scale_acro; /**< scale for ACRO mode */ - float rc_scale_yaw; + math::Vector<3> rc_scale; /**< scale for MANUAL mode */ } _params; /** @@ -275,6 +281,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.scale_acro.zero(); + _params.rc_scale.zero(); _R_sp.identity(); _R.identity(); @@ -286,21 +294,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); - - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_scale_acro = param_find("MC_ROLL_S_ACRO"); + _params_handles.pitch_scale_acro = param_find("MC_PITCH_S_ACRO"); + _params_handles.yaw_scale_acro = param_find("MC_YAW_S_ACRO"); + + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -344,6 +357,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_scale_acro, &v); + _params.scale_acro(0) = v; + param_get(_params_handles.rc_scale_roll, &v); + _params.rc_scale(0) = v; /* pitch */ param_get(_params_handles.pitch_p, &v); @@ -354,6 +371,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_scale_acro, &v); + _params.scale_acro(1) = v; + param_get(_params_handles.rc_scale_pitch, &v); + _params.rc_scale(1) = v; /* yaw */ param_get(_params_handles.yaw_p, &v); @@ -364,10 +385,11 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; - param_get(_params_handles.yaw_ff, &_params.yaw_ff); - - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + param_get(_params_handles.yaw_scale_acro, &v); + _params.scale_acro(2) = v; + param_get(_params_handles.rc_scale_yaw, &v); + _params.rc_scale(2) = v; return OK; } @@ -463,6 +485,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ + vehicle_manual_poll(); if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ @@ -488,16 +511,16 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2); + if (_params.rc_scale(2) > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2); if (_manual_control_sp.yaw > 0.0f) { yaw_sp_move_rate -= YAW_DEADZONE; } else { yaw_sp_move_rate += YAW_DEADZONE; } - yaw_sp_move_rate *= _params.rc_scale_yaw; + yaw_sp_move_rate *= _params.rc_scale(2); _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -743,7 +766,6 @@ MulticopterAttitudeControl::task_main() parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); - vehicle_manual_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -764,9 +786,37 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control, ACRO mode */ + vehicle_manual_poll(); + + _rates_sp(0) = _manual_control_sp.roll; + _rates_sp(1) = _manual_control_sp.pitch; + _rates_sp(2) = _manual_control_sp.yaw; + + /* rescale controls for ACRO mode */ + _rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro); + _thrust_sp = _manual_control_sp.throttle; + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } } if (_v_control_mode.flag_control_rates_enabled) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 27a45b6bb..f3a4022c8 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -54,3 +54,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a..e72b48a88 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -449,6 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index bd665b592..50ddec8a9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,6 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; + int rc_map_acro_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,6 +297,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; + param_t rc_map_acro_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -515,6 +517,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -669,6 +672,10 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("Failed getting acro sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } @@ -700,6 +707,7 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1290,6 +1298,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; + manual_control.acro_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; @@ -1428,6 +1437,11 @@ Sensors::rc_poll() manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* acro switch input */ + if (_rc.function[ACRO] >= 0) { + manual_control.acro_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ACRO]].scaled); + } + /* return switch input */ if (_rc.function[RETURN] >= 0) { manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..190dc01c8 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,6 +60,7 @@ struct manual_control_setpoint_s { 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 */ + float acro_switch; /**< acro 2 position switch (optional): normal, acro */ /** * Any of the channels below may not be available and be set to NaN diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef15..beb7008ab 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -68,11 +68,12 @@ enum RC_CHANNELS_FUNCTION ASSISTED = 6, MISSION = 7, OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, + ACRO = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1b3639e30..5cb0bd8a2 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,6 +63,7 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, + MAIN_STATE_ACRO, MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, @@ -116,6 +117,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + ACRO_SWITCH_NONE = 0, + ACRO_SWITCH_NORMAL, + ACRO_SWITCH_ACRO +} acro_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -192,6 +199,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + acro_switch_pos_t acro_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ -- cgit v1.2.3 From 8897894b19e8de4ad7960a0fa552ed12fc2f0200 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:14:21 +0100 Subject: commander, navigator, mc_att_control, mc_pos_control: code style fixed --- src/modules/commander/commander.cpp | 2 + src/modules/commander/state_machine_helper.cpp | 2 + src/modules/mc_att_control/mc_att_control_main.cpp | 6 ++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 11 +++++- src/modules/navigator/navigator_main.cpp | 5 ++- src/modules/navigator/navigator_mission.cpp | 44 ++++++++++++++-------- 6 files changed, 50 insertions(+), 20 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713..e9da69232 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -369,6 +369,7 @@ int arm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } @@ -381,6 +382,7 @@ int disarm() if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); return 0; + } else { return 1; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e..e5d77b246 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -382,6 +382,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_RTL: + /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { status->set_nav_state = NAV_STATE_RTL; @@ -392,6 +393,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f break; case FAILSAFE_STATE_LAND: + /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { status->set_nav_state = NAV_STATE_LAND; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0accb855..db5e2e9bb 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -489,14 +489,18 @@ MulticopterAttitudeControl::control_attitude(float dt) //} } else { float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; + if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + if (_manual_control_sp.yaw > 0.0f) { yaw_sp_move_rate -= YAW_DEADZONE; + } else { yaw_sp_move_rate += YAW_DEADZONE; } + yaw_sp_move_rate *= _params.rc_scale_yaw; _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; @@ -660,7 +664,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; if (isfinite(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT && - _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { + _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) { _rates_int(i) = rate_i; } } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 4fb9bd663..25d34c872 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -479,9 +479,11 @@ MulticopterPositionControl::select_alt(bool global) { if (global != _use_global_alt) { _use_global_alt = global; + if (global) { /* switch from barometric to global altitude */ _alt_sp += _global_pos.alt - _global_pos.baro_alt; + } else { /* switch from global to barometric altitude */ _alt_sp += _global_pos.baro_alt - _global_pos.alt; @@ -589,6 +591,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* select altitude source and update setpoint */ select_alt(_global_pos.global_valid); + if (!_use_global_alt) { alt = _global_pos.baro_alt; } @@ -845,9 +848,10 @@ MulticopterPositionControl::task_main() /* adjust limits for landing mode */ if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && - _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { + _pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { /* limit max tilt and min lift when landing */ tilt_max = _params.land_tilt_max; + if (thr_min < 0.0f) thr_min = 0.0f; } @@ -863,9 +867,11 @@ MulticopterPositionControl::task_main() if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); + if (thrust_sp_xy_len > 0.01f) { /* max horizontal thrust for given vertical thrust*/ float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max); + if (thrust_sp_xy_len > thrust_xy_max) { float k = thrust_xy_max / thrust_sp_xy_len; thrust_sp(0) *= k; @@ -874,15 +880,18 @@ MulticopterPositionControl::task_main() } } } + } else { /* thrust compensation for altitude only control mode */ float att_comp; if (_att.R[2][2] > TILT_COS_MAX) { att_comp = 1.0f / _att.R[2][2]; + } else if (_att.R[2][2] > 0.0f) { att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _att.R[2][2] + 1.0f; saturation_z = true; + } else { att_comp = 1.0f; saturation_z = true; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e961a8f94..6e4b5f0a0 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -689,7 +689,7 @@ Navigator::task_main() if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { /* switch to RTL if not already landed after RTL and home position set */ if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -747,7 +747,7 @@ Navigator::task_main() case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && (myState == NAV_STATE_READY || myState == NAV_STATE_LAND)) && - _vstatus.condition_home_position_valid) { + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } @@ -1575,6 +1575,7 @@ Navigator::on_mission_item_reached() if (_rtl_state == RTL_STATE_DESCEND) { /* hovering above home position, land if needed or loiter */ mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); + if (_mission_item.autocontinue) { dispatch(EVENT_LAND_REQUESTED); diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp index 6576aae70..e72caf98e 100644 --- a/src/modules/navigator/navigator_mission.cpp +++ b/src/modules/navigator/navigator_mission.cpp @@ -52,8 +52,8 @@ static const int ERROR = -1; -Mission::Mission() : - +Mission::Mission() : + _offboard_dataman_id(-1), _current_offboard_mission_index(0), _current_onboard_mission_index(0), @@ -65,7 +65,7 @@ Mission::Mission() : Mission::~Mission() { - + } void @@ -126,33 +126,39 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool { /* try onboard mission first */ if (current_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } + _current_mission_type = MISSION_TYPE_ONBOARD; *onboard = true; *index = _current_onboard_mission_index; - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (current_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ _current_mission_type = MISSION_TYPE_NONE; return ERROR; } + _current_mission_type = MISSION_TYPE_OFFBOARD; *onboard = false; *index = _current_offboard_mission_index; @@ -171,25 +177,29 @@ Mission::get_next_mission_item(struct mission_item_s *new_mission_item) { /* try onboard mission first */ if (next_onboard_mission_available()) { - + const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; } - /* otherwise fallback to offboard */ + /* otherwise fallback to offboard */ + } else if (next_offboard_mission_available()) { dm_item_t dm_current; if (_offboard_dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } const ssize_t len = sizeof(struct mission_item_s); + if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ return ERROR; @@ -244,14 +254,16 @@ void Mission::move_to_next() { switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - case MISSION_TYPE_NONE: - default: - break; + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } } \ No newline at end of file -- cgit v1.2.3 From 2923bdf39fd6e424523f0b6b47bef3cabcdc0645 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:25:29 +0100 Subject: commander: allow disarming in ACRO without landing (as in MANUAL) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89294fc76..30f75b9fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1120,7 +1120,7 @@ int commander_thread_main(int argc, char *argv[]) * 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.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { -- cgit v1.2.3 From 48f777d071652e382f930a4554ed809a60b7b0c5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 1 Feb 2014 13:54:39 +0100 Subject: commander sets vstatus.condition_landed = false for fw systems (until we have a landing detector): solves multiple issues with the state machine in the navigator app --- src/modules/commander/commander.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 9 +-------- 2 files changed, 9 insertions(+), 8 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e9da69232..aca3f6256 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -926,10 +926,12 @@ int commander_thread_main(int argc, char *argv[]) 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); + static bool published_condition_landed_fw = false; if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; + published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -938,6 +940,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { + if (!published_condition_landed_fw) { + status.condition_landed = false; // Fixedwing does not have a landing detector currently + published_condition_landed_fw = true; + status_changed = true; + } } /* update battery status */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 114776327..3ef2009c7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1443,14 +1443,7 @@ Navigator::check_mission_item_reached() } if (_mission_item.nav_cmd == NAV_CMD_LAND) { - if (_vstatus.is_rotary_wing) { - return _vstatus.condition_landed; - - } else { - /* For fw there is currently no landing detector: - * make sure control is not stopped when overshooting the landing waypoint */ - return false; - } + return _vstatus.condition_landed; } /* XXX TODO count turns */ -- cgit v1.2.3 From 3b2b270a40e0d8528339fe7cde5e0af91684fb97 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:55:12 +0100 Subject: mavlink: custom mode ACRO added --- src/modules/commander/px4_custom_mode.h | 1 + src/modules/mavlink/mavlink.c | 3 +++ 2 files changed, 4 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..d69ab3067 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_SEATBELT, PX4_CUSTOM_MAIN_MODE_EASY, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_ACRO, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f..22465a3da 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -212,6 +212,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u 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); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_ACRO) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; -- cgit v1.2.3 From 8ab94150db31e270b34cecaa4701628df873042c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 6 Feb 2014 20:23:23 +0100 Subject: commander: print_reject_mode() bug fixed --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 246efd61e..2c2b680b4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1624,7 +1624,7 @@ set_main_state_rc(struct vehicle_status_s *status) if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { res = main_state_transition(status, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode("OFFBOARD"); + print_reject_mode(status, "OFFBOARD"); } else { return res; -- cgit v1.2.3 From 6631ecf04a0592c816dfb832fa928a95877184d9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 11:45:35 +0100 Subject: commander: reset blink_msg_end when blink message completed to set normal LED status immediately --- src/modules/commander/commander_helper.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 033e7dc88..04624275f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -161,6 +161,7 @@ int blink_msg_state() return 0; } else if (hrt_absolute_time() > blink_msg_end) { + blink_msg_end = 0; return 2; } else { -- cgit v1.2.3 From 855944fb2eab33074537b5ccfc82e2462b662b5b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 00:24:40 +0100 Subject: commander: beeps and blinks cleanup --- .../commander/accelerometer_calibration.cpp | 2 +- src/modules/commander/airspeed_calibration.cpp | 2 +- src/modules/commander/commander.cpp | 46 +++++------- src/modules/commander/commander_helper.cpp | 86 +++++++++++++--------- src/modules/commander/commander_helper.h | 14 +--- 5 files changed, 76 insertions(+), 74 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 36b75dd58..1cbdf9bf8 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float (double)accel_ref[orient][2]); data_collected[orient] = true; - tune_neutral(); + tune_neutral(true); } close(sensor_combined_sub); diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 1809f9688..6039d92a7 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd) } mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(); + tune_neutral(true); close(diff_pres_sub); return OK; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b8573..193a7473e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1101,7 +1101,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; - tune_positive(); + tune_positive(true); } } @@ -1196,8 +1196,9 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine according to mode switches */ res = set_main_state_rc(&status); + /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { - tune_positive(); + tune_positive(armed.armed); } else if (res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ @@ -1253,7 +1254,7 @@ int commander_thread_main(int argc, char *argv[]) /* flight termination in manual mode if assisted switch is on easy position */ if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(); + tune_positive(armed.armed); } } @@ -1308,21 +1309,18 @@ 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_switch_available && safety.safety_off))) { /* play tune when armed */ - if (tune_arm() == OK) - arm_tune_played = true; + set_tune(TONE_ARMING_WARNING_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { /* play tune on battery warning */ - if (tune_low_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ - if (tune_critical_bat() == OK) - battery_tune_played = true; + set_tune(TONE_BATTERY_WARNING_FAST_TUNE); } else if (battery_tune_played) { - tune_stop(); + set_tune(TONE_STOP_TUNE); battery_tune_played = false; } @@ -1693,15 +1691,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg) sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - // only buzz if armed, because else we're driving people nuts indoors - // they really need to look at the leds as well. - if (status->arming_state == ARMING_STATE_ARMED) { - tune_negative(); - } else { - - // Always show the led indication - led_negative(); - } + /* only buzz if armed, because else we're driving people nuts indoors + they really need to look at the leds as well. */ + tune_negative(armed.armed); } } @@ -1715,7 +1707,7 @@ print_reject_arm(const char *msg) char s[80]; sprintf(s, "#audio: %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + tune_negative(true); } } @@ -1723,27 +1715,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); - tune_negative(); + tune_negative(true); break; case VEHICLE_CMD_RESULT_UNSUPPORTED: mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); - tune_negative(); + tune_negative(true); break; default: @@ -1883,9 +1875,9 @@ void *commander_low_prio_loop(void *arg) } if (calib_ret == OK) - tune_positive(); + tune_positive(true); else - tune_negative(); + tune_negative(true); arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 04624275f..265c0134c 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include @@ -82,10 +83,21 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) } static int buzzer; -static hrt_abstime blink_msg_end; +static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message +static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence +static int tune_current = 0; // currently playing tune, can be interrupted after tune_end +static int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { + tune_end = 0; + tune_current = 0; + memset(tune_durations, 0, sizeof(tune_durations)); + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; + buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); if (buzzer < 0) { @@ -93,6 +105,8 @@ int buzzer_init() return ERROR; } + ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); + return OK; } @@ -101,58 +115,60 @@ void buzzer_deinit() close(buzzer); } -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE); +void set_tune(int tune) { + int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ + if (tune_end == 0 || new_tune_duration > 0 || hrt_absolute_time() > tune_end) { + /* allow interrupting current non-repeating tune by the same tune */ + if (tune != tune_current || new_tune_duration > 0) { + ioctl(buzzer, TONE_SET_ALARM, tune); + } + tune_current = tune; + if (new_tune_duration > 0) { + tune_end = hrt_absolute_time() + new_tune_duration; + } else { + tune_end = 0; + } + } } -void tune_positive() +/** + * Blink green LED and play positive tune (if use_busser == true). + */ +void tune_positive(bool use_buzzer) { 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); + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } } -void tune_neutral() +/** + * Blink white LED and play neutral tune (if use_busser == true). + */ +void tune_neutral(bool use_buzzer) { 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() -{ - led_negative(); - ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); + } } -void led_negative() +/** + * Blink red LED and play negative tune (if use_busser == true). + */ +void tune_negative(bool use_buzzer) { blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); -} - -int tune_arm() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE); -} - -int tune_low_bat() -{ - return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE); -} - -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); + if (use_buzzer) { + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); + } } int blink_msg_state() diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index af25a5e97..e75f2592f 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status); int buzzer_init(void); void buzzer_deinit(void); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); -int tune_arm(void); -int tune_low_bat(void); -int tune_critical_bat(void); -void tune_stop(void); - -void led_negative(); +void set_tune(int tune); +void tune_positive(bool use_buzzer); +void tune_neutral(bool use_buzzer); +void tune_negative(bool use_buzzer); int blink_msg_state(); -- cgit v1.2.3 From 0613b299c033c21e1ddcbd8faeb9d3430d72386c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 09:13:51 +0100 Subject: commander: play warning tune (as for low battery) when in failsafe state --- src/modules/commander/commander.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 193a7473e..20bfca477 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,14 +1311,14 @@ int commander_thread_main(int argc, char *argv[]) /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) { - /* play tune on battery warning */ - set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* play tune on battery warning or failsafe */ + set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); + } else if (battery_tune_played) { set_tune(TONE_STOP_TUNE); battery_tune_played = false; @@ -1420,11 +1420,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a 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); - } - + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ } else { -- cgit v1.2.3 From 0ead560059fff0a31183f40a6b848e82aa2dae80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 11 Feb 2014 18:24:20 +0100 Subject: commander: tunes cleanup and fixes --- src/modules/commander/commander.cpp | 9 +++------ src/modules/commander/commander_helper.cpp | 32 ++++++++++++++---------------- 2 files changed, 18 insertions(+), 23 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 20bfca477..7284b38c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -607,7 +607,6 @@ int commander_thread_main(int argc, char *argv[]) /* not yet initialized */ commander_initialized = false; - bool battery_tune_played = false; bool arm_tune_played = false; /* set parameters */ @@ -1020,14 +1019,12 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; - battery_tune_played = false; } 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 */ critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; - battery_tune_played = false; if (armed.armed) { arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); @@ -1310,6 +1307,7 @@ int commander_thread_main(int argc, char *argv[]) if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) { /* play tune when armed */ set_tune(TONE_ARMING_WARNING_TUNE); + arm_tune_played = true; } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) { /* play tune on battery critical */ @@ -1319,13 +1317,12 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); - } else if (battery_tune_played) { + } else { set_tune(TONE_STOP_TUNE); - battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) { + if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) { arm_tune_played = false; } diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 265c0134c..fe6c9bfaa 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -82,20 +82,20 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status) || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } -static int buzzer; +static int buzzer = -1; static hrt_abstime blink_msg_end = 0; // end time for currently blinking LED message, 0 if no blink message static hrt_abstime tune_end = 0; // end time of currently played tune, 0 for repeating tunes or silence -static int tune_current = 0; // currently playing tune, can be interrupted after tune_end -static int tune_durations[TONE_NUMBER_OF_TUNES]; +static int tune_current = TONE_STOP_TUNE; // currently playing tune, can be interrupted after tune_end +static unsigned int tune_durations[TONE_NUMBER_OF_TUNES]; int buzzer_init() { tune_end = 0; tune_current = 0; memset(tune_durations, 0, sizeof(tune_durations)); - tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 700000; - tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 700000; + tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000; + tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000; + tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000; tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000; buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY); @@ -105,8 +105,6 @@ int buzzer_init() return ERROR; } - ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - return OK; } @@ -116,15 +114,15 @@ void buzzer_deinit() } void set_tune(int tune) { - int new_tune_duration = tune_durations[tune]; + unsigned int new_tune_duration = tune_durations[tune]; /* don't interrupt currently playing non-repeating tune by repeating */ - if (tune_end == 0 || new_tune_duration > 0 || hrt_absolute_time() > tune_end) { + if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ - if (tune != tune_current || new_tune_duration > 0) { + if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } tune_current = tune; - if (new_tune_duration > 0) { + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; } else { tune_end = 0; @@ -133,7 +131,7 @@ void set_tune(int tune) { } /** - * Blink green LED and play positive tune (if use_busser == true). + * Blink green LED and play positive tune (if use_buzzer == true). */ void tune_positive(bool use_buzzer) { @@ -146,7 +144,7 @@ void tune_positive(bool use_buzzer) } /** - * Blink white LED and play neutral tune (if use_busser == true). + * Blink white LED and play neutral tune (if use_buzzer == true). */ void tune_neutral(bool use_buzzer) { @@ -159,7 +157,7 @@ void tune_neutral(bool use_buzzer) } /** - * Blink red LED and play negative tune (if use_busser == true). + * Blink red LED and play negative tune (if use_buzzer == true). */ void tune_negative(bool use_buzzer) { @@ -185,8 +183,8 @@ int blink_msg_state() } } -static int leds; -static int rgbleds; +static int leds = -1; +static int rgbleds = -1; int led_init() { -- cgit v1.2.3 From 036ebdbe78defdcc8c1cf5955e8df773a16e7e8c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Feb 2014 16:08:49 +0100 Subject: Commander: add guard for parachute deployment --- src/modules/commander/commander.cpp | 14 +++++++++----- src/modules/navigator/navigator_params.c | 1 + 2 files changed, 10 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c039b8573..8129dddb3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -152,6 +152,7 @@ static uint64_t last_print_mode_reject_time = 0; static bool on_usb_power = false; static float takeoff_alt = 5.0f; +static int parachute_enabled = 0; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -563,7 +564,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command - if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + //XXX: to enable the parachute, a param needs to be set + //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO + if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -615,6 +618,7 @@ int commander_thread_main(int argc, char *argv[]) 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"); + param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); /* welcome user */ warnx("starting"); @@ -860,10 +864,10 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); - - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); } + /* navigation parameters */ + param_get(_param_takeoff_alt, &takeoff_alt); + param_get(_param_enable_parachute, ¶chute_enabled); } orb_check(sp_man_sub, &updated); @@ -1251,7 +1255,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(); } diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1ba159a8e..9e05bbffa 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -61,3 +61,4 @@ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment -- cgit v1.2.3 From 7441efde4745c0dddc08a36a0bbf83307f82948a Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Fri, 14 Feb 2014 01:48:00 +0100 Subject: Add a lot of MAVLink parameter documentation. --- src/lib/launchdetection/launchdetection_params.c | 48 ++-- src/modules/commander/commander_params.c | 32 +++ src/modules/mavlink/mavlink.c | 12 + src/modules/navigator/geofence_params.c | 16 +- src/modules/navigator/navigator_params.c | 63 ++++- src/modules/sensors/sensor_params.c | 301 ++++++++++++++++++++--- src/modules/systemlib/system_params.c | 19 +- 7 files changed, 432 insertions(+), 59 deletions(-) (limited to 'src/modules/commander') diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c index 63a8981aa..45d7957f1 100644 --- a/src/lib/launchdetection/launchdetection_params.c +++ b/src/lib/launchdetection/launchdetection_params.c @@ -45,28 +45,46 @@ #include /* - * Launch detection parameters, accessible via MAVLink + * Catapult launch detection parameters, accessible via MAVLink * */ -/* Catapult Launch detection */ - -// @DisplayName Switch to enable launchdetection -// @Description if set to 1 launchdetection is enabled -// @Range 0 or 1 +/** + * Enable launch detection. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); -// @DisplayName Catapult Accelerometer Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0 +/** + * Catapult accelerometer theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); -// @DisplayName Catapult Time Threshold -// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection -// @Range > 0, in seconds +/** + * Catapult time theshold. + * + * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection. + * + * @min 0 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); -// @DisplayName Throttle setting while detecting the launch -// @Description The throttle is set to this value while the system is waiting for the takeoff -// @Range 0 to 1 +/** + * Throttle setting while detecting launch. + * + * The throttle is set to this value while the system is waiting for the take-off. + * + * @min 0 + * @max 1 + * @group Launch detection + */ PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index d3155f7bf..80ca68f21 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -48,7 +48,39 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); + +/** + * Empty cell voltage. + * + * Defines the voltage where a single cell of the battery is considered empty. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f); + +/** + * Full cell voltage. + * + * Defines the voltage where a single cell of the battery is considered full. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f); + +/** + * Number of cells. + * + * Defines the number of cells the attached battery consists of. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); + +/** + * Battery capacity. + * + * Defines the capacity of the attached battery. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f..ade4469c5 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -76,8 +76,20 @@ #include /* define MAVLink specific parameters */ +/** + * MAVLink system ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_SYS_ID, 1); +/** + * MAVLink component ID + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_COMP_ID, 50); +/** + * MAVLink type + * @group MAVLink + */ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); __EXPORT int mavlink_main(int argc, char *argv[]); diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 20dd1fe2f..5831a0ca9 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -45,11 +45,17 @@ #include /* - * geofence parameters, accessible via MAVLink - * + * Geofence parameters, accessible via MAVLink */ -// @DisplayName Switch to enable geofence -// @Description if set to 1 geofence is enabled, defaults to 1 because geofence is only enabled when the geofence.txt file is present -// @Range 0 or 1 +/** + * Enable geofence. + * + * Set to 1 to enable geofence. + * Defaults to 1 because geofence is only enabled when the geofence.txt file is present. + * + * @min 0 + * @max 1 + * @group Geofence + */ PARAM_DEFINE_INT32(GF_ON, 1); diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9e05bbffa..ec7a4e229 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,15 +50,72 @@ /* * Navigator parameters, accessible via MAVLink - * */ +/** + * Minimum altitude + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); + +/** + * Waypoint acceptance radius. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); + +/** + * Loiter radius. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); + +/** + * @group Navigation + */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); + +/** + * Default take-off altitude. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude + +/** + * Landing altitude. + * + * Slowly descend from this altitude when landing. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing + +/** + * Return-to-land altitude. + * + * Minimum altitude for going home in RTL mode. + * + * @group Navigation + */ PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); // delay after descend before landing, if set to -1 the system will not land but loiter at NAV_LAND_ALT -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); // enable parachute deployment + +/** + * Return-to-land delay. + * + * Delay after descend before landing. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @group Navigation + */ +PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); + +/** + * Enable parachute deployment. + * + * @group Navigation + */ +PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a..288c6e00a 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -42,13 +42,10 @@ */ #include - #include /** - * Gyro X offset - * - * This is an X-axis offset for the gyro. Adjust it according to the calibration data. + * Gyro X-axis offset * * @min -10.0 * @max 10.0 @@ -57,7 +54,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); /** - * Gyro Y offset + * Gyro Y-axis offset * * @min -10.0 * @max 10.0 @@ -66,7 +63,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); /** - * Gyro Z offset + * Gyro Z-axis offset * * @min -5.0 * @max 5.0 @@ -75,9 +72,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); /** - * Gyro X scaling - * - * X-axis scaling. + * Gyro X-axis scaling factor * * @min -1.5 * @max 1.5 @@ -86,9 +81,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); /** - * Gyro Y scaling - * - * Y-axis scaling. + * Gyro Y-axis scaling factor * * @min -1.5 * @max 1.5 @@ -97,9 +90,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); /** - * Gyro Z scaling - * - * Z-axis scaling. + * Gyro Z-axis scaling factor * * @min -1.5 * @max 1.5 @@ -107,10 +98,9 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); + /** - * Magnetometer X offset - * - * This is an X-axis offset for the magnetometer. + * Magnetometer X-axis offset * * @min -500.0 * @max 500.0 @@ -119,9 +109,7 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); /** - * Magnetometer Y offset - * - * This is an Y-axis offset for the magnetometer. + * Magnetometer Y-axis offset * * @min -500.0 * @max 500.0 @@ -130,9 +118,7 @@ PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f); PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); /** - * Magnetometer Z offset - * - * This is an Z-axis offset for the magnetometer. + * Magnetometer Z-axis offset * * @min -500.0 * @max 500.0 @@ -140,24 +126,134 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f); +/** + * Magnetometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_XSCALE, 1.0f); + +/** + * Magnetometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); + +/** + * Magnetometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); + +/** + * Accelerometer X-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XOFF, 0.0f); + +/** + * Accelerometer Y-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YOFF, 0.0f); + +/** + * Accelerometer Z-axis offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZOFF, 0.0f); +/** + * Accelerometer X-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); + +/** + * Accelerometer Y-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); + +/** + * Accelerometer Z-axis scaling factor + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); + +/** + * Differential pressure sensor offset + * + * @group Sensor Calibration + */ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); + +/** + * Differential pressure sensor analog enabled + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); + +/** + * Board rotation + * + * This parameter defines the rotation of the FMU board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); + +/** + * External magnetometer rotation + * + * This parameter defines the rotation of the external magnetometer relative + * to the platform (not relative to the FMU). + * See SENS_BOARD_ROT for possible values. + * + * @group Sensor Calibration + */ PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0); + /** * RC Channel 1 Minimum * @@ -367,20 +463,52 @@ PARAM_DEFINE_FLOAT(RC18_DZ, 0.0f); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ #endif -PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ +/** + * DSM binding trigger. + * + * -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind + * + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_DSM_BIND, -1); + + +/** + * Scaling factor for battery voltage sensor on PX4IO. + * + * @group Battery Calibration + */ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); + #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 +/** + * Scaling factor for battery voltage sensor on FMU v2. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); #else -/* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ -/* PX4IOAR: 0.00838095238 */ -/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +/** + * Scaling factor for battery voltage sensor on FMU v1. + * + * FMUv1 standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 + * FMUv1 with PX4IO: 0.00459340659 + * FMUv1 with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) = 0.00838095238 + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif + +/** + * Scaling factor for battery current sensor. + * + * @group Battery Calibration + */ PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ + /** * Roll control channel mapping. * @@ -446,22 +574,127 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); + +/** + * Return switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); + +/** + * Assist switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); + +/** + * Mission switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Flaps channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); -PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */ +/** + * Auxiliary switch 1 channel mapping. + * + * Default function: Camera pitch + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); + +/** + * Auxiliary switch 2 channel mapping. + * + * Default function: Camera roll + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ -PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */ +/** + * Auxiliary switch 3 channel mapping. + * + * Default function: Camera azimuth / yaw + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); + + +/** + * Roll scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f); + +/** + * Pitch scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f); + +/** + * Yaw scaling factor + * + * @group Radio Calibration + */ PARAM_DEFINE_FLOAT(RC_SCALE_YAW, 2.0f); -PARAM_DEFINE_INT32(RC_FS_CH, 0); /**< RC failsafe channel, 0 = disable */ -PARAM_DEFINE_INT32(RC_FS_MODE, 0); /**< RC failsafe mode: 0 = too low means signal loss, 1 = too high means signal loss */ -PARAM_DEFINE_FLOAT(RC_FS_THR, 800); /**< RC failsafe PWM threshold */ + +/** + * Failsafe channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_CH, 0); + +/** + * Failsafe channel mode. + * + * 0 = too low means signal loss, + * 1 = too high means signal loss + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_FS_MODE, 0); + +/** + * Failsafe channel PWM threshold. + * + * @min 0 + * @max 1 + * @group Radio Calibration + */ +PARAM_DEFINE_FLOAT(RC_FS_THR, 800); diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 75be090f8..cb35a2541 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -40,8 +40,23 @@ #include #include -// Auto-start script with index #n +/** + * Auto-start script index. + * + * Defines the auto-start script used to bootstrap the system. + * + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOSTART, 0); -// Automatically configure default values +/** + * Automatically configure default values. + * + * Set to 1 to set platform-specific parameters to their default + * values on next system startup. + * + * @min 0 + * @max 1 + * @group System + */ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0); -- cgit v1.2.3 From 66f270dd7e2a8668ae7a4f3387e5098381eff84d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 14 Feb 2014 13:39:57 +0100 Subject: commander: double subscribing to offboard setpoint fixed --- src/modules/commander/commander.cpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c2b680b4..bf7bf2ef3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -761,11 +761,6 @@ int commander_thread_main(int argc, char *argv[]) struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); - /* Subscribe to offboard control data */ - int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s offboard_sp; - memset(&offboard_sp, 0, sizeof(offboard_sp)); - /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); struct offboard_control_setpoint_s sp_offboard; @@ -883,10 +878,6 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - if (updated) { - orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); - } - orb_check(sp_offboard_sub, &updated); if (updated) { @@ -1285,7 +1276,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check offboard signal */ - if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (sp_offboard.timestamp != 0 && hrt_absolute_time() < sp_offboard.timestamp + OFFBOARD_TIMEOUT) { if (!status.offboard_control_signal_found_once) { status.offboard_control_signal_found_once = true; mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); @@ -1301,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; if (status.main_state == MAIN_STATE_OFFBOARD) { - if (offboard_sp.armed && !armed.armed) { + if (sp_offboard.armed && !armed.armed) { if (!safety.safety_switch_available || safety.safety_off) { transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1310,7 +1301,7 @@ int commander_thread_main(int argc, char *argv[]) } } - } else if (!offboard_sp.armed && armed.armed) { + } else if (!sp_offboard.armed && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); -- cgit v1.2.3 From 44c095b9e5717c707faa3528b861bad8eb7ac94a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 19:46:57 +0100 Subject: commander: allow arming from RC with safety enabled in HIL mode --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8129dddb3..6d14472f3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -430,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -516,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe 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) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -1156,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { + if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { -- cgit v1.2.3 From e8bee868e2386b5ce9fc2bc4b8ca6aa63f240232 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 16:50:00 +0400 Subject: commander: publish vehicle_control_mode for OFFBOARD state --- src/modules/commander/commander.cpp | 32 ++++++++++++++++++++++---- src/modules/mavlink/mavlink_receiver.cpp | 1 + src/modules/uORB/topics/vehicle_control_mode.h | 1 + 3 files changed, 30 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1b9ca6988..61ef63921 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -159,6 +159,7 @@ static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; +static struct offboard_control_setpoint_s sp_offboard; /* tasks waiting for low prio thread */ typedef enum { @@ -767,7 +768,6 @@ int commander_thread_main(int argc, char *argv[]) /* 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 */ @@ -1691,6 +1691,8 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = false; control_mode.flag_control_termination_enabled = false; /* set this flag when navigator should act */ @@ -1701,7 +1703,6 @@ set_control_mode() switch (status.main_state) { case MAIN_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = status.is_rotary_wing; control_mode.flag_control_attitude_enabled = status.is_rotary_wing; control_mode.flag_control_altitude_enabled = false; @@ -1712,7 +1713,6 @@ set_control_mode() case MAIN_STATE_SEATBELT: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; @@ -1723,7 +1723,6 @@ set_control_mode() case MAIN_STATE_EASY: control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; @@ -1735,6 +1734,31 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + case MAIN_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_offboard_enabled = false; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + + default: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + + break; + default: break; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0ca64853a..735bc50da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -330,6 +330,7 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } + // TODO use vehicle_control_mode.flag_control_offboard_enabled if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 7cbb37cd8..5444c4ebf 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -74,6 +74,7 @@ struct vehicle_control_mode_s bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_auto_enabled; /**< true if onboard autopilot should act */ + bool flag_control_offboard_enabled; /**< true if offboard control should be used */ 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 horizontal velocity (implies direction) is controlled */ -- cgit v1.2.3 From 172c82c258819c6f2d95b85b56c41015f928b282 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 17:21:43 +0400 Subject: commander: vehicle_control_mode fixes --- src/modules/commander/commander.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f82c8c35b..1ef872e39 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1736,7 +1736,7 @@ set_control_mode() case MAIN_STATE_OFFBOARD: control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_offboard_enabled = false; + control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -1776,7 +1776,6 @@ set_control_mode() case FAILSAFE_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_position_enabled = false; -- cgit v1.2.3 From 3b270fbdab60c0fac1c76dff170b0a9b6f494e8d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 18:07:12 +0400 Subject: commander: auto arm/disarm by offboard thrust "feature" removed --- src/modules/commander/commander.cpp | 20 -------------------- 1 file changed, 20 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ef872e39..4a69318c9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1295,26 +1295,6 @@ int commander_thread_main(int argc, char *argv[]) status.offboard_control_signal_lost = false; - if (status.main_state == MAIN_STATE_OFFBOARD) { - if (sp_offboard.armed && !armed.armed) { - if (!safety.safety_switch_available || safety.safety_off) { - transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); - } - } - - } else if (!sp_offboard.armed && armed.armed) { - arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); - - if (res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); - } - } - } - } else { if (!status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST"); -- cgit v1.2.3 From 8bc0287eccc7871db17c199a3b330e74c92c068a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 22:45:54 +0400 Subject: commander: disarm system when safety enabled --- src/modules/commander/commander.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6d14472f3..29a930ebb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -902,11 +902,13 @@ 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, ARMING_STATE_STANDBY, &armed); - // } + /* disarm if safety is now on and still armed */ + if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + } + } } /* update global position estimate */ -- cgit v1.2.3 From 4e27fd9a381bd32ba5b79d275528ac19d1fb9442 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 26 Feb 2014 00:22:19 +0400 Subject: commander/px4_custom_mode.h: added missing include --- src/modules/commander/px4_custom_mode.h | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..2144d3460 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -8,6 +8,8 @@ #ifndef PX4_CUSTOM_MODE_H_ #define PX4_CUSTOM_MODE_H_ +#include + enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, PX4_CUSTOM_MAIN_MODE_SEATBELT, -- cgit v1.2.3 From 0656aae3cb46f405888d8130e9db4bb3f9dd529a Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 14:31:23 +0000 Subject: Now setting flag_external_manual_override_ok to false when in offboard mode,so that fmu has control over outputs. Added handling of OFFBOARD_CONTROL_MODE_DIRECT_RATES. --- src/modules/commander/commander.cpp | 11 ++++++++++- src/modules/mavlink/mavlink_receiver.cpp | 18 ++++++++++++++++++ 2 files changed, 28 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f0debef1c..5e242050b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1728,8 +1728,17 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; + control_mode.flag_external_manual_override_ok = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_external_manual_override_ok = false; break; - default: control_mode.flag_control_rates_enabled = false; control_mode.flag_control_attitude_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6c0eb169b..ba886e29e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -98,6 +98,7 @@ static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_attitude_setpoint_s att_sp; +static struct vehicle_rates_setpoint_s rates_sp; static struct vehicle_control_mode_s _v_control_mode; struct vehicle_global_position_s hil_global_pos; @@ -130,6 +131,7 @@ static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t att_sp_pub = -1; +static orb_advert_t rates_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; @@ -343,6 +345,22 @@ handle_message(mavlink_message_t *msg) // if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + rates_sp.timestamp = hrt_absolute_time(); + rates_sp.roll = offboard_control_sp.p1; + rates_sp.pitch = offboard_control_sp.p2; + rates_sp.yaw = offboard_control_sp.p3; + rates_sp.thrust = offboard_control_sp.p4; + + /* check if topic has to be advertised */ + if(rates_sp_pub <= 0) { + rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } + break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: att_sp.timestamp = hrt_absolute_time(); att_sp.roll_body = offboard_control_sp.p1; -- cgit v1.2.3 From be4af0ab3bd1b4313ba23cbe4c7e2702d56d3644 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 17:07:29 +0000 Subject: code clean up --- src/examples/jetdrive_control/jetdrive_control_main.cpp | 2 +- src/modules/commander/commander.cpp | 2 -- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/examples/jetdrive_control/jetdrive_control_main.cpp b/src/examples/jetdrive_control/jetdrive_control_main.cpp index 409ccad63..01435f572 100644 --- a/src/examples/jetdrive_control/jetdrive_control_main.cpp +++ b/src/examples/jetdrive_control/jetdrive_control_main.cpp @@ -558,7 +558,7 @@ JetdriveControl::task_main() arming_status_poll(); vehicle_manual_poll(); - if(_v_control_mode.flag_external_manual_override_ok) + if(!_v_control_mode.flag_control_offboard_enabled) { /* manual/direct control */ _actuators.control[0] = _manual_control_sp .roll; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5e242050b..801ef8d01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1728,7 +1728,6 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; - control_mode.flag_external_manual_override_ok = false; break; case OFFBOARD_CONTROL_MODE_DIRECT_RATES: control_mode.flag_control_rates_enabled = true; @@ -1737,7 +1736,6 @@ set_control_mode() control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; - control_mode.flag_external_manual_override_ok = false; break; default: control_mode.flag_control_rates_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ba886e29e..a834b2c3a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -358,7 +358,7 @@ handle_message(mavlink_message_t *msg) } else { /* publish */ - orb_publish(ORB_ID(vehicle_rates_setpoint), &rates_sp); + orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: -- cgit v1.2.3 From 3674aae4a746099a162cbf943311b587cd46822f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 21:19:56 +0400 Subject: commander: ignore battery voltage in HIL mode --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d5af63120..483437029 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -962,7 +962,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; -- cgit v1.2.3 From b76e26c5e5f37c4fb086a68e0427d9d297e8d225 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 3 Mar 2014 21:24:59 +0400 Subject: commander: allow arming with safety enabled in HIL mode --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 483437029..d114a2e5c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -902,7 +902,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(safety), safety_sub, &safety); /* disarm if safety is now on and still armed */ - if (safety.safety_switch_available && !safety.safety_off && armed.armed) { + if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); -- cgit v1.2.3 From 8818425e584c9670dfa9a823b3dfd95147c087d4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Mar 2014 18:25:09 +0400 Subject: commander: fixed message formatting when disabling sensors publishing in HIL mode --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5..bf49403d0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -350,7 +350,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); -- cgit v1.2.3 From 772fce9f82749bde8c14199fcc73cfa2235e760e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:38:55 +0100 Subject: commander: Improve HIL publication blocking call handling --- src/modules/commander/state_machine_helper.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5..131d6a550 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -309,10 +309,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s bool valid_transition = false; int ret = ERROR; - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); valid_transition = true; } else { @@ -347,10 +344,16 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + + if (sensfd < 0) { + warn("failed opening device"); + return 1; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "FAIL"); } closedir(d); -- cgit v1.2.3 From 8383603f7691e3bc66534b58654c4d13547632a2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:41:03 +0100 Subject: commander: Linting: Reduce scope of variable --- src/modules/commander/state_machine_helper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 131d6a550..5537c4e80 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -337,10 +337,11 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; - struct dirent *direntry; d = opendir("/dev"); if (d) { + struct dirent *direntry; + while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); -- cgit v1.2.3 From 6db2191a71ed0ea38e1fd18886371f099c14d593 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 18:50:20 +0100 Subject: commander: Skip devices we do not want to touch in HIL --- src/modules/commander/state_machine_helper.cpp | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1e892ed77..9178bcc5a 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -351,6 +351,23 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s return 1; } + /* skip serial ports */ + if (!strncmp("tty", direntry-d_name, 3)) { + continue; + } + /* skip mtd devices */ + if (!strncmp("mtd", direntry-d_name, 3)) { + continue; + } + /* skip ram devices */ + if (!strncmp("ram", direntry-d_name, 3)) { + continue; + } + /* skip mavlink */ + if (!strcmp("mavlink", direntry-d_name)) { + continue; + } + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); @@ -359,8 +376,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s closedir(d); - warnx("directory listing ok (FS mounted and readable)"); - } else { /* failed opening dir */ warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); -- cgit v1.2.3 From 2fe9f65c45ba3aaad549b30e3b34568d07ab2b20 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 16 Mar 2014 19:16:18 +0100 Subject: commander: Fixed compile error and some stupidity in usage of path names --- src/modules/commander/state_machine_helper.cpp | 40 ++++++++++++++++++-------- 1 file changed, 28 insertions(+), 12 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 9178bcc5a..e6f245cf9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include #include @@ -341,37 +342,52 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (d) { struct dirent *direntry; + char devname[24]; while ((direntry = readdir(d)) != NULL) { - int sensfd = ::open(direntry->d_name, 0); - - if (sensfd < 0) { - warn("failed opening device"); - return 1; - } - /* skip serial ports */ - if (!strncmp("tty", direntry-d_name, 3)) { + if (!strncmp("tty", direntry->d_name, 3)) { continue; } /* skip mtd devices */ - if (!strncmp("mtd", direntry-d_name, 3)) { + if (!strncmp("mtd", direntry->d_name, 3)) { continue; } /* skip ram devices */ - if (!strncmp("ram", direntry-d_name, 3)) { + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { continue; } /* skip mavlink */ - if (!strcmp("mavlink", direntry-d_name)) { + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); continue; } int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s: %s\n", direntry->d_name, (block_ret == OK) ? "OK" : "ERROR"); + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); -- cgit v1.2.3 From dec13e7f215caa3ee002fbc3b6400ae82e249b2e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Mar 2014 21:38:23 -0700 Subject: Use single set of arm/disarm code Set mode and arm/disarm commands now call a single method to arm disarm, thus removing code duplication. Also calls updated arming_state_transition method such that the arming state logic does not need to be duplicated outside of arming_state_transition. --- src/modules/commander/commander.cpp | 115 +++++++++++------------------------- 1 file changed, 34 insertions(+), 81 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5c..e010c233f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -137,7 +137,7 @@ enum MAV_MODE_FLAG { }; /* Mavlink file descriptors */ -static int mavlink_fd; +static int mavlink_fd = 0; /* flags */ static bool commander_initialized = false; @@ -217,11 +217,10 @@ void print_reject_arm(const char *msg); void print_status(); -int arm(); -int disarm(); - 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); +transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -288,12 +287,12 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - arm(); + armDisarm(true, mavlink_fd, "command line"); exit(0); } - if (!strcmp(argv[1], "disarm")) { - disarm(); + if (!strcmp(argv[1], "2")) { + armDisarm(false, mavlink_fd, "command line"); exit(0); } @@ -363,30 +362,22 @@ void print_status() static orb_advert_t status_pub; -int arm() +transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy) { - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; - - } else { - return 1; - } -} - -int disarm() -{ - int arming_res = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by commandline"); - return 0; - - } else { - return 1; - } + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will + // output appropriate error messages if the state cannot transition. + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + if (arming_res == TRANSITION_CHANGED && mavlink_fd) { + char buf[256]; + snprintf(buf, sizeof(buf), "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_fd, buf); + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; } bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) @@ -424,37 +415,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (hil_ret == OK) ret = true; - // TODO remove debug code - //mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode); - /* set arming state */ - arming_res = TRANSITION_NOT_CHANGED; - - if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_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"); - } - - } 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); - - if (arming_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); - } - - } else { - arming_res = TRANSITION_NOT_CHANGED; - } - } + // Transition the arming state + arming_res = armDisarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); if (arming_res == TRANSITION_CHANGED) ret = true; @@ -513,26 +475,17 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } 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 && status->hil_state == HIL_STATE_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_critical(mavlink_fd, "#audio: ARMED by component arm cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; - - } else { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + // Follow exactly what the mavlink spec says for values + if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) { + return VEHICLE_CMD_RESULT_UNSUPPORTED; + } + + transition_result_t arming_res = armDisarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; } } break; -- cgit v1.2.3 From 4b980b508c93f8bb840ababf19e61862dcedbda9 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 16 Mar 2014 21:39:06 -0700 Subject: arming_state_transition now outputs error messages If mavlink fd is passed in method will output reason for arming failure to mavlink. --- src/modules/commander/state_machine_helper.cpp | 21 +++++++++++++-------- src/modules/commander/state_machine_helper.h | 2 +- 2 files changed, 14 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5..94ffc31d6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,7 +70,7 @@ static bool failsafe_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_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) { /* * Perform an atomic state update @@ -125,13 +125,18 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * 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) - && (!safety->safety_switch_available || safety->safety_off || status->hil_state == HIL_STATE_ON)) { /* only allow arming if safety is off */ - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; - } + if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE)) { + if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } + } else { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = true; + } + } break; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f04879ff9..0ddd4f05a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,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, const int mavlink_fd = 0); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -- cgit v1.2.3 From 783a2403969032e8c0119196cb94c092847965d1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 17 Mar 2014 11:01:15 -0700 Subject: Function naming and remove unneeded sprintf --- src/modules/commander/commander.cpp | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e010c233f..1c11e2587 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -219,7 +219,7 @@ void print_status(); 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); -transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy); +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -287,12 +287,12 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "arm")) { - armDisarm(true, mavlink_fd, "command line"); + arm_disarm(true, mavlink_fd, "command line"); exit(0); } if (!strcmp(argv[1], "2")) { - armDisarm(false, mavlink_fd, "command line"); + arm_disarm(false, mavlink_fd, "command line"); exit(0); } @@ -362,7 +362,7 @@ void print_status() static orb_advert_t status_pub; -transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; @@ -370,9 +370,7 @@ transition_result_t armDisarm(bool arm, const int mavlink_fd, const char* armedB // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - char buf[256]; - snprintf(buf, sizeof(buf), "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); - mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } @@ -416,7 +414,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe ret = true; // Transition the arming state - arming_res = armDisarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); if (arming_res == TRANSITION_CHANGED) ret = true; @@ -480,7 +478,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe return VEHICLE_CMD_RESULT_UNSUPPORTED; } - transition_result_t arming_res = armDisarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; -- cgit v1.2.3 From 83da4ae02dfc61d6a7f80ae40660826fbbca81be Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 27 Mar 2014 00:27:11 +0400 Subject: 'vehicle_global_position' topic updated: removed baro_alt and XXX_valid flags. --- src/drivers/frsky_telemetry/frsky_data.c | 2 +- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 1 - .../attitude_estimator_ekf_main.cpp | 2 +- src/modules/commander/commander.cpp | 73 ++++++++++------------ src/modules/mavlink/mavlink_receiver.cpp | 3 +- src/modules/navigator/navigator_main.cpp | 17 +++-- .../position_estimator_inav_main.c | 48 +++++++------- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sdlog2/sdlog2_messages.h | 6 +- src/modules/uORB/topics/vehicle_global_position.h | 7 +-- 10 files changed, 75 insertions(+), 88 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index cfcf91e3f..57a03bc84 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -225,7 +225,7 @@ void frsky_send_frame2(int uart) float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; char lat_ns = 0, lon_ew = 0; int sec = 0; - if (global_pos.global_valid) { + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { time_t time_gps = global_pos.time_gps_usec / 1000000; struct tm *tm_gps = gmtime(&time_gps); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 03e6021dc..5cf84542b 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -312,7 +312,6 @@ void KalmanNav::updatePublications() // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; _pos.lat = lat * M_RAD_TO_DEG; _pos.lon = lon * M_RAD_TO_DEG; _pos.alt = float(alt); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 10a6cd2c5..c61b6ff3f 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -407,7 +407,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && global_pos.global_valid && hrt_absolute_time() < global_pos.timestamp + 500000) { + } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5c..7da062961 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ 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 POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define POSITION_TIMEOUT 20000 /**< consider the local or global position estimate invalid after 20ms */ #define RC_TIMEOUT 100000 #define DIFFPRESS_TIMEOUT 2000000 @@ -919,7 +919,37 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.global_valid, &(status.condition_global_position_valid), &status_changed); + check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed); + + /* check if GPS fix is ok */ + static float hdop_threshold_m = 4.0f; + static float vdop_threshold_m = 8.0f; + + /* update home position */ + if (!status.condition_home_position_valid && updated && + (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m) && + (hrt_absolute_time() < global_position.timestamp + POSITION_TIMEOUT) && !armed.armed) { + + /* copy position data to uORB home message, store it locally as well */ + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); + + /* 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 */ + status.condition_home_position_valid = true; + tune_positive(true); + } /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -1067,45 +1097,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); - /* check if GPS fix is ok */ - 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 (!status.condition_home_position_valid && gps_position.fix_type >= 3 && - (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { - - /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; - - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); - - /* 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 */ - status.condition_home_position_valid = true; - tune_positive(true); - } } /* start RC input check */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6eec25e4b..2c9cdbf24 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -765,7 +765,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) memset(&hil_global_pos, 0, sizeof(hil_global_pos)); hil_global_pos.timestamp = timestamp; - hil_global_pos.global_valid = true; hil_global_pos.lat = hil_state.lat; hil_global_pos.lon = hil_state.lon; hil_global_pos.alt = hil_state.alt / 1000.0f; @@ -773,6 +772,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) hil_global_pos.vel_e = hil_state.vy / 100.0f; hil_global_pos.vel_d = hil_state.vz / 100.0f; hil_global_pos.yaw = hil_attitude.yaw; + hil_global_pos.eph = 2.0f; + hil_global_pos.epv = 4.0f; if (_global_pos_pub > 0) { orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &hil_global_pos); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..ef7201790 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -177,7 +177,7 @@ private: class Mission _mission; bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ + bool _global_pos_valid; /**< track changes of global_position */ bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; @@ -817,13 +817,11 @@ Navigator::task_main() if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { _pos_sp_triplet_updated = true; - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { + if (myState == NAV_STATE_LAND && !_global_pos_valid) { /* got global position when landing, update setpoint */ start_land(); } - _global_pos_valid = _global_pos.global_valid; - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { if (check_mission_item_reached()) { @@ -846,8 +844,15 @@ Navigator::task_main() /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } + + _global_pos_valid = true; + + } else { + /* assume that global position is valid if updated in last 20ms */ + _global_pos_valid = _global_pos.timestamp != 0 && hrt_abstime() < _global_pos.timestamp + 20000; } + /* publish position setpoint triplet if updated */ if (_pos_sp_triplet_updated) { _pos_sp_triplet_updated = false; @@ -893,9 +898,9 @@ Navigator::start() void Navigator::status() { - warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in"); + warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - if (_global_pos.global_valid) { + if (_global_pos_valid) { warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); warnx("Altitude %5.5f meters, altitude above home %5.5f meters", (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); 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 4f7147167..3f1a5d39b 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -292,7 +292,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); - orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t vehicle_global_position_pub = -1; struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; @@ -340,7 +340,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; - global_pos.baro_valid = true; } } } @@ -550,9 +549,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* reproject position estimate to new reference */ float dx, dy; map_projection_project(&ref, home.lat, home.lon, &dx, &dy); - est_x[0] -= dx; - est_y[0] -= dx; - est_z[0] += home.alt - local_pos.ref_alt; + x_est[0] -= dx; + y_est[0] -= dx; + z_est[0] += home.alt - local_pos.ref_alt; } /* update baro offset */ @@ -888,40 +887,35 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - /* publish global position */ - global_pos.global_valid = local_pos.xy_global; + if (local_pos.xy_global && local_pos.z_global) { + /* publish global position */ + global_pos.timestamp = t; + global_pos.time_gps_usec = gps.time_gps_usec; - if (local_pos.xy_global) { double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = est_lat; global_pos.lon = est_lon; - global_pos.time_gps_usec = gps.time_gps_usec; - } + global_pos.alt = local_pos.ref_alt - local_pos.z; - /* set valid values even if position is not valid */ - if (local_pos.v_xy_valid) { global_pos.vel_n = local_pos.vx; global_pos.vel_e = local_pos.vy; - } - - if (local_pos.z_global) { - global_pos.alt = local_pos.ref_alt - local_pos.z; - } - - if (local_pos.z_valid) { - global_pos.baro_alt = baro_offset - local_pos.z; - } - - if (local_pos.v_z_valid) { global_pos.vel_d = local_pos.vz; - } - global_pos.yaw = local_pos.yaw; + global_pos.yaw = local_pos.yaw; - global_pos.timestamp = t; + // TODO implement dead-reckoning + global_pos.eph = gps.eph_m; + global_pos.epv = gps.epv_m; - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + if (vehicle_global_position_pub < 0) { + vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + + } else { + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } + } } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c7073eb94..36d309d6c 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1126,8 +1126,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_GPOS.vel_n = buf.global_pos.vel_n; log_msg.body.log_GPOS.vel_e = buf.global_pos.vel_e; log_msg.body.log_GPOS.vel_d = buf.global_pos.vel_d; - log_msg.body.log_GPOS.baro_alt = buf.global_pos.baro_alt; - log_msg.body.log_GPOS.flags = (buf.global_pos.baro_valid ? 1 : 0) | (buf.global_pos.global_valid ? 2 : 0); + log_msg.body.log_GPOS.eph = buf.global_pos.eph; + log_msg.body.log_GPOS.epv = buf.global_pos.epv; LOGBUFFER_WRITE_AND_COUNT(GPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index e27518aa0..fbfca76f7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -204,8 +204,8 @@ struct log_GPOS_s { float vel_n; float vel_e; float vel_d; - float baro_alt; - uint8_t flags; + float eph; + float epv; }; /* --- GPSP - GLOBAL POSITION SETPOINT --- */ @@ -317,7 +317,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), - LOG_FORMAT(GPOS, "LLfffffB", "Lat,Lon,Alt,VelN,VelE,VelD,BaroAlt,Flags"), + LOG_FORMAT(GPOS, "LLffffff", "Lat,Lon,Alt,VelN,VelE,VelD,EPH,EPV"), LOG_FORMAT(GPSP, "BLLffBfbf", "NavState,Lat,Lon,Alt,Yaw,Type,LoitR,LoitDir,PitMin"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index ff9e98e1c..5c54630e2 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -63,9 +63,6 @@ struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - bool global_valid; /**< true if position satisfies validity criteria of estimator */ - bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ @@ -74,8 +71,8 @@ struct vehicle_global_position_s float vel_e; /**< Ground east velocity, m/s */ float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float eph; + float epv; }; /** -- cgit v1.2.3 From 521539897e59f495adaae481911e3a4ac847625e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 26 Mar 2014 14:51:57 -0700 Subject: Simpler state transition code Also fixed ARMING_STATE_ARMED_ERROR->ARMING_STATE_STANDBY_ERROR transition. --- src/modules/commander/state_machine_helper.cpp | 175 +++++++++++-------------- src/modules/uORB/topics/vehicle_status.h | 2 + 2 files changed, 76 insertions(+), 101 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 94ffc31d6..8564747fa 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -68,10 +68,44 @@ static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; +// This array defines the arming state transitions. The rows are the new state, and the columns +// are the current state. Using new state and current state you can index into the array which +// will be true for a valid transition or false for a invalid transition. In some cases even +// though the transition is marked as true additional checks must be made. See arming_state_transition +// code for those checks. +static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI +}; + +// You can index into the array with an arming_state_t in order to get it's textual representation +static const char* state_names[ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", +}; + 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, const int mavlink_fd) +arming_state_transition(struct vehicle_status_s *status, /// current vehicle status + const struct safety_s *safety, /// current safety settings + arming_state_t new_arming_state, /// arming state requested + struct actuator_armed_s *armed, /// current armed status + const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { + // Double check that our static arrays are still valid + ASSERT(ARMING_STATE_INIT == 0); + ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + /* * Perform an atomic state update */ @@ -82,121 +116,60 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* only check transition if the new state is actually different from the current one */ if (new_arming_state == status->arming_state) { ret = TRANSITION_NOT_CHANGED; - } else { - /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; - } else { armed->lockdown = false; } - - switch (new_arming_state) { - case ARMING_STATE_INIT: - - /* 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; - } - - break; - - case ARMING_STATE_STANDBY: - - /* allow coming from INIT and disarming from ARMED */ - if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED - || status->hil_state == HIL_STATE_ON) { - - /* sensors need to be initialized for STANDBY state */ - if (status->condition_system_sensors_initialized) { - ret = TRANSITION_CHANGED; - armed->armed = false; - armed->ready_to_arm = true; - } - } - - 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)) { + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == ARMING_STATE_ARMED) { + // In non-HIL, fail transition if we need safety switch press if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - // If we need to wait for safety switch then output message, but only if we have fd for mavlink connection if (mavlink_fd) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); } - } else { - ret = TRANSITION_CHANGED; - armed->armed = true; - armed->ready_to_arm = true; + valid_transition = false; } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; } - - 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; - } - - break; - - 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) { - status->arming_state = new_arming_state; - arming_state_changed = true; - } - } - + } + + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; + } + + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) - warnx("arming transition rejected"); + if (ret == TRANSITION_DENIED) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "Invalid arming transition from %s to %s", state_names[status->arming_state], state_names[new_arming_state]); + } + warnx("arming transition rejected"); + } return ret; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 5d40554fd..0071d3125 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -69,6 +69,8 @@ typedef enum { MAIN_STATE_MAX } main_state_t; +// If you change the order, add or remove arming_state_t states make sure to update the arrays +// in state_machine_helper.cpp as well. typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, -- cgit v1.2.3 From 66527eea02ca6c8b1e33047f60bce1b832f82071 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Mar 2014 17:54:29 +0100 Subject: commander: workaround to prevent RC loss in HIL --- src/modules/commander/commander.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d114a2e5c..cf7ba757e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define RC_TIMEOUT_HIL 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1108,8 +1109,16 @@ int commander_thread_main(int argc, char *argv[]) } } + + /* + * XXX workaround: + * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) + * which can trigger RC loss if the computer/simulator lags. + */ + uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; + /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && 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; -- cgit v1.2.3 From b9a56fbeb9effad0b30e2085dba4ea92041456be Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Mar 2014 13:05:51 -0700 Subject: Fixed bug with transition from in air restore to armed Also added better debug output for invalid transitions --- src/modules/commander/state_machine_helper.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8564747fa..359cc62cd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -129,8 +129,10 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == ARMING_STATE_ARMED) { - // In non-HIL, fail transition if we need safety switch press - if (status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + // Fail transition if we need safety switch press + // Allow if coming from in air restore + // Allow if HIL_STATE_ON + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { if (mavlink_fd) { mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); } @@ -165,10 +167,11 @@ arming_state_transition(struct vehicle_status_s *status, /// current irqrestore(flags); if (ret == TRANSITION_DENIED) { + static const char* errMsg = "Invalid arming transition from %s to %s"; if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "Invalid arming transition from %s to %s", state_names[status->arming_state], state_names[new_arming_state]); + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); } - warnx("arming transition rejected"); + warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); } return ret; -- cgit v1.2.3 From dcc11b8cabda82ff8616ee2d14db9a53d6d23ac0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Mar 2014 13:06:09 -0700 Subject: Update to convert to C++ style --- src/modules/commander/commander_tests/commander_tests.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 6e72cf0d9..0abb84a82 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,8 +48,7 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - state_machine_helper_test(); - //state_machine_test(); + stateMachineHelperTest(); return 0; } -- cgit v1.2.3 From f97263f5a094e9112ecdf4a5af99ad39d4a5dda4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Thu, 27 Mar 2014 13:07:27 -0700 Subject: Added comprehensive arming_state_transition unit test Also converted full to C++ style. Also converted to new unit test code. --- .../commander_tests/state_machine_helper_test.cpp | 357 +++++++++++++++------ .../commander_tests/state_machine_helper_test.h | 2 +- 2 files changed, 267 insertions(+), 92 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 40bedd9f3..8a946543d 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,13 +49,12 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual const char* run_tests(); + virtual void runTests(void); private: - const char* arming_state_transition_test(); - const char* arming_state_transition_arm_disarm_test(); - const char* main_state_transition_test(); - const char* is_safe_test(); + bool armingStateTransitionTest(); + bool mainStateTransitionTest(); + bool isSafeTest(); }; StateMachineHelperTest::StateMachineHelperTest() { @@ -64,61 +63,242 @@ StateMachineHelperTest::StateMachineHelperTest() { StateMachineHelperTest::~StateMachineHelperTest() { } -const char* -StateMachineHelperTest::arming_state_transition_test() +bool StateMachineHelperTest::armingStateTransitionTest(void) { + // These are the critical values from vehicle_status_s and actuator_armed_s which must be primed + // to simulate machine state prior to testing an arming state transition. This structure is also + // use to represent the expected machine state after the transition has been requested. + typedef struct { + arming_state_t arming_state; // vehicle_status_s.arming_state + bool armed; // actuator_armed_s.armed + bool ready_to_arm; // actuator_armed_s.ready_to_arm + } ArmingTransitionVolatileState_t; + + // This structure represents a test case for arming_state_transition. It contains the machine + // state prior to transtion, the requested state to transition to and finally the expected + // machine state after transition. + typedef struct { + const char* assertMsg; // Text to show when test case fails + ArmingTransitionVolatileState_t current_state; // Machine state prior to transtion + hil_state_t hil_state; // Current vehicle_status_s.hil_state + bool condition_system_sensors_initialized; // Current vehicle_status_s.condition_system_sensors_initialized + bool safety_switch_available; // Current safety_s.safety_switch_available + bool safety_off; // Current safety_s.safety_off + arming_state_t requested_state; // Requested arming state to transition to + ArmingTransitionVolatileState_t expected_state; // Expected machine state after transition + transition_result_t expected_transition_result; // Expected result from arming_state_transition + } ArmingTransitionTest_t; + + // We use these defines so that our test cases are more readable + #define ATT_ARMED true + #define ATT_DISARMED false + #define ATT_READY_TO_ARM true + #define ATT_NOT_READY_TO_ARM false + #define ATT_SENSORS_INITIALIZED true + #define ATT_SENSORS_NOT_INITIALIZED false + #define ATT_SAFETY_AVAILABLE true + #define ATT_SAFETY_NOT_AVAILABLE true + #define ATT_SAFETY_OFF true + #define ATT_SAFETY_ON false + + // These are test cases for arming_state_transition + static const ArmingTransitionTest_t rgArmingTransitionTests[] = { + // TRANSITION_NOT_CHANGED tests + + { "no transition: identical states", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_NOT_CHANGED }, + + // TRANSITION_CHANGED tests + + // Check all basic valid transitions, these don't require special state in vehicle_status_t or safety_s + + { "transition: init to standby", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to standby error", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to reboot", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to init", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to standby error", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby to reboot", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed to standby", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed to armed error", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED_ERROR, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: armed error to standby error", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY_ERROR, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: standby error to reboot", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: in air restore to armed", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: in air restore to reboot", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // hil on tests, standby error to standby not normally allowed + + { "transition: standby error to standby, hil on", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_ON, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // Safety switch arming tests + + { "transition: init to standby, no safety switch", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + { "transition: init to standby, safety switch off", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // standby error + { "transition: armed error to standby error requested standby", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_CHANGED }, + + // TRANSITION_DENIED tests + + // Check some important basic invalid transitions, these don't require special state in vehicle_status_t or safety_s + + { "no transition: init to armed", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby to armed error", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED_ERROR, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed to init", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_INIT, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed to reboot", + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed error to armed", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: armed error to reboot", + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_REBOOT, + { ARMING_STATE_ARMED_ERROR, ATT_ARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby error to armed", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: standby error to standby", + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_STANDBY_ERROR, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: reboot to armed", + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_REBOOT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + { "no transition: in air restore to standby", + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_IN_AIR_RESTORE, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + // Sensor tests + + { "no transition: init to standby - sensors not initialized", + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_NOT_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_STANDBY, + { ARMING_STATE_INIT, ATT_DISARMED, ATT_NOT_READY_TO_ARM }, TRANSITION_DENIED }, + + // Safety switch arming tests + + { "no transition: init to standby, safety switch on", + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_ON, + ARMING_STATE_ARMED, + { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, TRANSITION_DENIED }, + }; + struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; - struct actuator_armed_s armed; - - // Identical states. - status.arming_state = ARMING_STATE_INIT; - new_arming_state = ARMING_STATE_INIT; - mu_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - - // INIT to STANDBY. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = true; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("transition: init to standby", - TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: standby", ARMING_STATE_STANDBY == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("ready to arm", armed.ready_to_arm); - - // INIT to STANDBY, sensors not initialized. - armed.armed = false; - armed.ready_to_arm = false; - status.arming_state = ARMING_STATE_INIT; - status.condition_system_sensors_initialized = false; - new_arming_state = ARMING_STATE_STANDBY; - mu_assert("no transition: sensors not initialized", - TRANSITION_DENIED == arming_state_transition(&status, &safety, new_arming_state, &armed)); - mu_assert("current state: init", ARMING_STATE_INIT == status.arming_state); - mu_assert("not armed", !armed.armed); - mu_assert("not ready to arm", !armed.ready_to_arm); - - return 0; -} - -const char* -StateMachineHelperTest::arming_state_transition_arm_disarm_test() -{ - struct vehicle_status_s status; - struct safety_s safety; - arming_state_t new_arming_state; + struct safety_s safety; struct actuator_armed_s armed; - - // TODO(sjwilks): ARM then DISARM. - return 0; + + size_t cArmingTransitionTests = sizeof(rgArmingTransitionTests) / sizeof(rgArmingTransitionTests[0]); + for (size_t i=0; icurrent_state.arming_state; + status.condition_system_sensors_initialized = test->condition_system_sensors_initialized; + status.hil_state = test->hil_state; + safety.safety_switch_available = test->safety_switch_available; + safety.safety_off = test->safety_off; + armed.armed = test->current_state.armed; + armed.ready_to_arm = test->current_state.ready_to_arm; + + // Attempt transition + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed); + + // Validate result of transition + ut_assert(test->assertMsg, test->expected_transition_result == result); + ut_assert(test->assertMsg, status.arming_state == test->expected_state.arming_state); + ut_assert(test->assertMsg, armed.armed == test->expected_state.armed); + ut_assert(test->assertMsg, armed.ready_to_arm == test->expected_state.ready_to_arm); + } + + return true; } -const char* -StateMachineHelperTest::main_state_transition_test() +bool StateMachineHelperTest::mainStateTransitionTest(void) { struct vehicle_status_s current_state; main_state_t new_main_state; @@ -126,70 +306,69 @@ StateMachineHelperTest::main_state_transition_test() // Identical states. current_state.main_state = MAIN_STATE_MANUAL; new_main_state = MAIN_STATE_MANUAL; - mu_assert("no transition: identical states", + ut_assert("no transition: identical states", TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // AUTO to MANUAL. current_state.main_state = MAIN_STATE_AUTO; new_main_state = MAIN_STATE_MANUAL; - mu_assert("transition changed: auto to manual", + ut_assert("transition changed: auto to manual", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to SEATBELT. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; new_main_state = MAIN_STATE_SEATBELT; - mu_assert("tranisition: manual to seatbelt", + ut_assert("tranisition: manual to seatbelt", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); // MANUAL to SEATBELT, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; new_main_state = MAIN_STATE_SEATBELT; - mu_assert("no transition: invalid local altitude", + ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to EASY. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; new_main_state = MAIN_STATE_EASY; - mu_assert("transition: manual to easy", + ut_assert("transition: manual to easy", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); // MANUAL to EASY, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; new_main_state = MAIN_STATE_EASY; - mu_assert("no transition: invalid position", + ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); // MANUAL to AUTO. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = true; new_main_state = MAIN_STATE_AUTO; - mu_assert("transition: manual to auto", + ut_assert("transition: manual to auto", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); + ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); // MANUAL to AUTO, invalid global position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_global_position_valid = false; new_main_state = MAIN_STATE_AUTO; - mu_assert("no transition: invalid global position", + ut_assert("no transition: invalid global position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - mu_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - return 0; + return true; } -const char* -StateMachineHelperTest::is_safe_test() +bool StateMachineHelperTest::isSafeTest(void) { struct vehicle_status_s current_state; struct safety_s safety; @@ -199,49 +378,45 @@ StateMachineHelperTest::is_safe_test() armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: not armed", is_safe(¤t_state, &safety, &armed)); armed.armed = false; armed.lockdown = true; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: software lockdown", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = true; - mu_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: safety off", !is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = true; safety.safety_off = false; - mu_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); + ut_assert("is safe: safety off", is_safe(¤t_state, &safety, &armed)); armed.armed = true; armed.lockdown = false; safety.safety_switch_available = false; safety.safety_off = false; - mu_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); + ut_assert("not safe: no safety switch", !is_safe(¤t_state, &safety, &armed)); - return 0; + return true; } -const char* -StateMachineHelperTest::run_tests() +void StateMachineHelperTest::runTests(void) { - mu_run_test(arming_state_transition_test); - mu_run_test(arming_state_transition_arm_disarm_test); - mu_run_test(main_state_transition_test); - mu_run_test(is_safe_test); - - return 0; + ut_run_test(armingStateTransitionTest); + ut_run_test(mainStateTransitionTest); + ut_run_test(isSafeTest); } -void -state_machine_helper_test() +void stateMachineHelperTest(void) { StateMachineHelperTest* test = new StateMachineHelperTest(); - test->UnitTest::print_results(test->run_tests()); + test->runTests(); + test->printResults(); } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index 10a68e602..bbf66255e 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void state_machine_helper_test(); +void stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ -- cgit v1.2.3 From 63cd319ff73ddf6bcbf15d3e35afd5df7b58d72e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 11:57:41 +0400 Subject: commander: set home position on arming --- src/modules/commander/commander.cpp | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bc63c810b..410906c26 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -612,6 +612,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; + bool was_armed = false; /* set parameters */ param_t _param_sys_type = param_find("MAV_TYPE"); @@ -927,17 +928,15 @@ int commander_thread_main(int argc, char *argv[]) static float vdop_threshold_m = 8.0f; /* update home position */ - if (!status.condition_home_position_valid && updated && - (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m) && - (hrt_absolute_time() < global_position.timestamp + POSITION_TIMEOUT) && !armed.armed) { + if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && + (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { - /* copy position data to uORB home message, store it locally as well */ home.lat = global_position.lat; home.lon = global_position.lon; home.alt = global_position.alt; - warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { @@ -1284,7 +1283,32 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); + + /* update home position on arming */ + if (armed.armed && !was_armed && status.condition_global_position_valid && + (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { + + // TODO remove code duplication + home.lat = global_position.lat; + home.lon = global_position.lon; + home.alt = global_position.alt; + + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + + /* 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 */ + status.condition_home_position_valid = true; + } } + was_armed = armed.armed; if (main_state_changed) { status_changed = true; -- cgit v1.2.3 From b1d39e65a61ec17d2da30ad37068758ab23d3ba3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 15:36:11 +0400 Subject: commander: position timeout increased to 30ms --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 410906c26..1e072678b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ 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 POSITION_TIMEOUT 20000 /**< consider the local or global position estimate invalid after 20ms */ +#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ #define RC_TIMEOUT 100000 #define RC_TIMEOUT_HIL 500000 #define DIFFPRESS_TIMEOUT 2000000 -- cgit v1.2.3 From 553b122830615b1617570900cf5f5d0c04720c8b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 16:53:22 +0400 Subject: caommander: setting home position by command implemented --- src/modules/commander/commander.cpp | 51 ++++++++++++++++++++++++++++++++++--- 1 file changed, 48 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1e072678b..a896f6146 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -195,7 +195,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -390,7 +390,7 @@ int disarm() } } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -580,6 +580,51 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; + case VEHICLE_CMD_DO_SET_HOME: { + bool use_current = cmd->param1 > 0.5f; + if (use_current) { + /* use current position */ + if (status->condition_global_position_valid) { + home->lat = global_pos->lat; + home->lon = global_pos->lon; + home->alt = global_pos->alt; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + } else { + /* use specified position */ + home->lat = cmd->param5; + home->lon = cmd->param6; + home->alt = cmd->param7; + + home->timestamp = hrt_absolute_time(); + + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); + + /* 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 */ + status->condition_home_position_valid = true; + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -1268,7 +1313,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed)) + if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) status_changed = true; } -- cgit v1.2.3 From 93617c4073d560ec2a804d728a2830534a74a50a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 2 Apr 2014 17:09:18 +0400 Subject: commander: set home position on arming only if at least 2 s from commander start spent --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a896f6146..7257cb4b3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1329,8 +1329,8 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] arming state: %s", arming_states_str[status.arming_state]); - /* update home position on arming */ - if (armed.armed && !was_armed && status.condition_global_position_valid && + /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { // TODO remove code duplication -- cgit v1.2.3 From 367ce63b86b863d72a3f6b4250d9da780a85f40b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 11:45:57 +0400 Subject: 'signal_lost' flag added to manual_control_setpoint and rc_channels topics to indicate signal loss immediately --- src/modules/commander/commander.cpp | 13 +- src/modules/sensors/sensors.cpp | 224 ++++++++++++---------- src/modules/uORB/topics/manual_control_setpoint.h | 2 + src/modules/uORB/topics/rc_channels.h | 1 + 4 files changed, 123 insertions(+), 117 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..edd77231d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && !sp_man.signal_lost && 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; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f890c4c7f..9f816f5e1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -474,6 +474,7 @@ Sensors::Sensors() : _battery_discharged(0), _battery_current_timestamp(0) { + memset(&_rc, 0, sizeof(_rc)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -1283,12 +1284,6 @@ Sensors::rc_poll() if (rc_updated) { /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ struct rc_input_values rc_input; - - orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); - - if (rc_input.rc_lost) - return; - struct manual_control_setpoint_s manual_control; struct actuator_controls_s actuator_group_3; @@ -1311,19 +1306,28 @@ Sensors::rc_poll() manual_control.aux4 = NAN; manual_control.aux5 = NAN; - /* require at least four channels to consider the signal valid */ - if (rc_input.channel_count < 4) - return; + manual_control.signal_lost = true; - /* failsafe check */ - if (_parameters.rc_fs_ch != 0) { - if (_parameters.rc_fs_mode == 0) { - if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) - return; + orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input); + + /* detect RC signal loss */ + /* check flags and require at least four channels to consider the signal valid */ + if (!(rc_input.rc_lost || rc_input.rc_failsafe || rc_input.channel_count < 4)) { + /* signal looks good */ + manual_control.signal_lost = false; + + /* check throttle failsafe */ + if (_parameters.rc_fs_ch != 0) { + if (_parameters.rc_fs_mode == 0) { + if (rc_input.values[_parameters.rc_fs_ch - 1] < _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } - } else if (_parameters.rc_fs_mode == 1) { - if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) - return; + } else if (_parameters.rc_fs_mode == 1) { + if (rc_input.values[_parameters.rc_fs_ch - 1] > _parameters.rc_fs_thr) { + manual_control.signal_lost = true; + } + } } } @@ -1332,10 +1336,7 @@ Sensors::rc_poll() if (channel_limit > _rc_max_chan_count) channel_limit = _rc_max_chan_count; - /* we are accepting this message */ - _rc_last_valid = rc_input.timestamp_last_signal; - - /* Read out values from raw message */ + /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { /* @@ -1382,105 +1383,124 @@ Sensors::rc_poll() } _rc.chan_count = rc_input.channel_count; - _rc.timestamp = rc_input.timestamp_last_signal; + _rc.rssi = rc_input.rssi; + _rc.signal_lost = manual_control.signal_lost; - manual_control.timestamp = rc_input.timestamp_last_signal; + if (!manual_control.signal_lost) { + _rc_last_valid = rc_input.timestamp_last_signal; + } - /* roll input - rolling right is stick-wise and rotation-wise positive */ - manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); - /* - * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, - * so reverse sign. - */ - manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); - /* yaw input - stick right is positive and positive rotation */ - manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); - /* throttle input */ - manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; + _rc.timestamp = _rc_last_valid; - if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; + manual_control.timestamp = _rc_last_valid; - if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; + if (!manual_control.signal_lost) { + /* fill values in manual_control_setpoint topic only if signal is valid */ - /* scale output */ - if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { - manual_control.roll *= _parameters.rc_scale_roll; - } + /* roll input - rolling right is stick-wise and rotation-wise positive */ + manual_control.roll = limit_minus_one_to_one(_rc.chan[_rc.function[ROLL]].scaled); + /* + * pitch input - stick down is negative, but stick down is pitching up (pos) in NED, + * so reverse sign. + */ + manual_control.pitch = limit_minus_one_to_one(-1.0f * _rc.chan[_rc.function[PITCH]].scaled); + /* yaw input - stick right is positive and positive rotation */ + manual_control.yaw = limit_minus_one_to_one(_rc.chan[_rc.function[YAW]].scaled); + /* throttle input */ + manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled; - if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { - manual_control.pitch *= _parameters.rc_scale_pitch; - } + if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { - manual_control.yaw *= _parameters.rc_scale_yaw; - } + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; - /* flaps */ - if (_rc.function[FLAPS] >= 0) { + /* scale output */ + if (isfinite(_parameters.rc_scale_roll) && _parameters.rc_scale_roll > 0.0f) { + manual_control.roll *= _parameters.rc_scale_roll; + } - manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); + if (isfinite(_parameters.rc_scale_pitch) && _parameters.rc_scale_pitch > 0.0f) { + manual_control.pitch *= _parameters.rc_scale_pitch; + } - if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { - manual_control.flaps *= _parameters.rc_scale_flaps; + if (isfinite(_parameters.rc_scale_yaw) && _parameters.rc_scale_yaw > 0.0f) { + manual_control.yaw *= _parameters.rc_scale_yaw; } - } - /* mode switch input */ - if (_rc.function[MODE] >= 0) { - manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); - } + /* flaps */ + if (_rc.function[FLAPS] >= 0) { - /* assisted switch input */ - if (_rc.function[ASSISTED] >= 0) { - manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); - } + manual_control.flaps = limit_minus_one_to_one(_rc.chan[_rc.function[FLAPS]].scaled); - /* mission switch input */ - if (_rc.function[MISSION] >= 0) { - manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); - } + if (isfinite(_parameters.rc_scale_flaps) && _parameters.rc_scale_flaps > 0.0f) { + manual_control.flaps *= _parameters.rc_scale_flaps; + } + } - /* return switch input */ - if (_rc.function[RETURN] >= 0) { - manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - } + /* mode switch input */ + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* assisted switch input */ + if (_rc.function[ASSISTED] >= 0) { + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + } - /* aux functions, only assign if valid mapping is present */ - if (_rc.function[AUX_1] >= 0) { - manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); - } + /* mission switch input */ + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + } - if (_rc.function[AUX_2] >= 0) { - manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); - } + /* return switch input */ + if (_rc.function[RETURN] >= 0) { + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + } - if (_rc.function[AUX_3] >= 0) { - manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); - } + // if (_rc.function[OFFBOARD_MODE] >= 0) { + // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + // } - if (_rc.function[AUX_4] >= 0) { - manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); - } + /* aux functions, only assign if valid mapping is present */ + if (_rc.function[AUX_1] >= 0) { + manual_control.aux1 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_1]].scaled); + } + + if (_rc.function[AUX_2] >= 0) { + manual_control.aux2 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_2]].scaled); + } + + if (_rc.function[AUX_3] >= 0) { + manual_control.aux3 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_3]].scaled); + } - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + if (_rc.function[AUX_4] >= 0) { + manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); + } + + if (_rc.function[AUX_5] >= 0) { + manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); + } + + /* copy from mapped manual control to control group 3 */ + actuator_group_3.control[0] = manual_control.roll; + actuator_group_3.control[1] = manual_control.pitch; + actuator_group_3.control[2] = manual_control.yaw; + actuator_group_3.control[3] = manual_control.throttle; + actuator_group_3.control[4] = manual_control.flaps; + actuator_group_3.control[5] = manual_control.aux1; + actuator_group_3.control[6] = manual_control.aux2; + actuator_group_3.control[7] = manual_control.aux3; + + /* publish actuator_controls_3 topic only if control signal is valid */ + if (_actuator_group_3_pub > 0) { + orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); + + } else { + _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); + } } - /* copy from mapped manual control to control group 3 */ - actuator_group_3.control[0] = manual_control.roll; - actuator_group_3.control[1] = manual_control.pitch; - actuator_group_3.control[2] = manual_control.yaw; - actuator_group_3.control[3] = manual_control.throttle; - actuator_group_3.control[4] = manual_control.flaps; - actuator_group_3.control[5] = manual_control.aux1; - actuator_group_3.control[6] = manual_control.aux2; - actuator_group_3.control[7] = manual_control.aux3; - - /* check if ready for publishing */ + /* publish rc_channels topic even if signal is invalid, for debug */ if (_rc_pub > 0) { orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc); @@ -1489,21 +1509,13 @@ Sensors::rc_poll() _rc_pub = orb_advertise(ORB_ID(rc_channels), &_rc); } - /* check if ready for publishing */ + /* publish manual_control_setpoint topic even if signal is not valid to update 'signal_lost' flag */ if (_manual_control_pub > 0) { orb_publish(ORB_ID(manual_control_setpoint), _manual_control_pub, &manual_control); } else { _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } - - /* check if ready for publishing */ - if (_actuator_group_3_pub > 0) { - orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3); - - } else { - _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); - } } } diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..5d1384380 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -51,6 +51,8 @@ struct manual_control_setpoint_s { uint64_t timestamp; + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ + float roll; /**< ailerons roll / roll rate input */ float pitch; /**< elevator / pitch / pitch rate */ float yaw; /**< rudder / yaw rate / yaw */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 6eb20efd1..3246a39dd 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -94,6 +94,7 @@ struct rc_channels_s { char function_name[RC_CHANNELS_FUNCTION_MAX][20]; int8_t function[RC_CHANNELS_FUNCTION_MAX]; uint8_t rssi; /**< Overall receive signal strength */ + bool signal_lost; /**< control signal lost, should be checked together with topic timeout */ }; /**< radio control channels. */ /** -- cgit v1.2.3 From 6f38ed3b4b6f266098d0616b6bd3c18ffe082755 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 3 Apr 2014 20:23:34 +0400 Subject: commander, navigator: use updated manual_control_setpoint --- src/modules/commander/commander.cpp | 136 ++++++++++++++----------------- src/modules/navigator/navigator_main.cpp | 100 ++++++++--------------- src/modules/uORB/topics/vehicle_status.h | 28 ------- 3 files changed, 95 insertions(+), 169 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edd77231d..7e469c9c1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -112,8 +113,7 @@ extern struct system_load_s system_load; #define MAVLINK_OPEN_INTERVAL 50000 -#define STICK_ON_OFF_LIMIT 0.75f -#define STICK_THRUST_RANGE 1.0f +#define STICK_ON_OFF_LIMIT 0.9f #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) @@ -207,7 +207,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status); -transition_result_t set_main_state_rc(struct vehicle_status_s *status); +transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man); void set_control_mode(); @@ -814,6 +814,11 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); + /* Subscribe to position setpoint triplet */ + int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); + struct position_setpoint_triplet_s pos_sp_triplet; + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -1005,6 +1010,13 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + /* update subsystem */ + orb_check(pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; @@ -1135,7 +1147,7 @@ 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1153,7 +1165,7 @@ 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 && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); @@ -1193,11 +1205,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } - /* fill status according to mode switches */ - check_mode_switches(&sp_man, &status); - /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status); + res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ if (res == TRANSITION_CHANGED) { @@ -1208,6 +1217,33 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAV_STATE_RTL; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAV_STATE_LOITER; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + + } else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE && + pos_sp_triplet.nav_state == NAV_STATE_RTL) { + /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + status.set_nav_state = NAV_STATE_MISSION; + status.set_nav_state_timestamp = hrt_absolute_time(); + } + } + } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); @@ -1255,7 +1291,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1470,76 +1506,26 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a leds_counter++; } -void -check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status) -{ - /* main mode switch */ - if (!isfinite(sp_man->mode_switch)) { - /* default to manual if signal is invalid */ - status->mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_AUTO; - - } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { - status->mode_switch = MODE_SWITCH_MANUAL; - - } else { - status->mode_switch = MODE_SWITCH_ASSISTED; - } - - /* return switch */ - if (!isfinite(sp_man->return_switch)) { - status->return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - status->return_switch = RETURN_SWITCH_RETURN; - - } else { - status->return_switch = RETURN_SWITCH_NORMAL; - } - - /* assisted switch */ - if (!isfinite(sp_man->assisted_switch)) { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { - status->assisted_switch = ASSISTED_SWITCH_EASY; - - } else { - status->assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* mission switch */ - if (!isfinite(sp_man->mission_switch)) { - status->mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { - status->mission_switch = MISSION_SWITCH_LOITER; - - } else { - status->mission_switch = MISSION_SWITCH_MISSION; - } -} - transition_result_t -set_main_state_rc(struct vehicle_status_s *status) +set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; - switch (status->mode_switch) { - case MODE_SWITCH_MANUAL: + switch (sp_man->mode_switch) { + case SWITCH_POS_NONE: + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_ASSISTED: - if (status->assisted_switch == ASSISTED_SWITCH_EASY) { + case SWITCH_POS_MIDDLE: // ASSISTED + if (sp_man->assisted_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT print_reject_mode(status, "EASY"); @@ -1547,29 +1533,33 @@ set_main_state_rc(struct vehicle_status_s *status) res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode + } - if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + if (sp_man->assisted_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; - case MODE_SWITCH_AUTO: + case SWITCH_POS_ON: // AUTO res = main_state_transition(status, MAIN_STATE_AUTO); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to SEATBELT (EASY likely will not work too) print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_SEATBELT); - if (res != TRANSITION_DENIED) + if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state + } // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c45cafc1b..b7f26198b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -690,84 +690,46 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state machine from commander and set the navigator mode accordingly */ + /* evaluate state requested by commander */ if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - bool stick_mode = false; - - if (!_vstatus.rc_signal_lost) { - /* RC signal available, use control switches to set mode */ - /* RETURN switch, overrides MISSION switch */ - if (_vstatus.return_switch == RETURN_SWITCH_RETURN) { - /* switch to RTL if not already landed after RTL and home position set */ + if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { + /* commander requested new navigation mode, try to set it */ + switch (_vstatus.set_nav_state) { + case NAV_STATE_NONE: + /* nothing to do */ + break; + + case NAV_STATE_LOITER: + request_loiter_or_ready(); + break; + + case NAV_STATE_MISSION: + request_mission_if_available(); + break; + + case NAV_STATE_RTL: if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { + (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + _vstatus.condition_home_position_valid) { dispatch(EVENT_RTL_REQUESTED); } - stick_mode = true; + break; - } else { - /* MISSION switch */ - if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - request_loiter_or_ready(); - stick_mode = true; + case NAV_STATE_LAND: + dispatch(EVENT_LAND_REQUESTED); - } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - request_mission_if_available(); - stick_mode = true; - } + break; - if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - request_mission_if_available(); - stick_mode = true; - } + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - } - - if (!stick_mode) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } + } else { + /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ + if (myState == NAV_STATE_NONE) { + request_mission_if_available(); } } @@ -775,6 +737,8 @@ Navigator::task_main() /* navigator shouldn't act */ dispatch(EVENT_NONE_REQUESTED); } + + _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; } /* parameters updated */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 56be4d241..48af4c9e2 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -93,29 +93,6 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; -typedef enum { - MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, - 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_NORMAL, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - -typedef enum { - MISSION_SWITCH_NONE = 0, - MISSION_SWITCH_LOITER, - MISSION_SWITCH_MISSION -} mission_switch_pos_t; - enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -187,11 +164,6 @@ struct vehicle_status_s { bool is_rotary_wing; - 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; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; -- cgit v1.2.3 From ed7b97c0203fd1a16768222cad6c6f49b0534bd2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:13:03 +0200 Subject: commander: don't beep if message is not understood --- src/modules/commander/commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..58995fcec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -592,11 +592,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); - } + /* silently ignore unsupported commands, maybe they are passed on over mavlink */ +// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { +// /* already warned about unsupported commands in "default" case */ +// answer_command(*cmd, result); +// } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From 88cf841f00b499792780195de63018b3bd49f683 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 4 Apr 2014 18:18:17 +0200 Subject: Bump RC timeout for all cases to half a second --- src/modules/commander/commander.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cf7ba757e..3720b5a3b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -118,8 +118,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ -#define RC_TIMEOUT 100000 -#define RC_TIMEOUT_HIL 500000 +#define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -1109,16 +1108,8 @@ int commander_thread_main(int argc, char *argv[]) } } - - /* - * XXX workaround: - * Prevent RC loss in HIL when sensors.cpp is only publishing sp_man at a low rate (e.g. 30Hz) - * which can trigger RC loss if the computer/simulator lags. - */ - uint64_t rc_timeout = status.hil_state == HIL_STATE_ON ? RC_TIMEOUT_HIL : RC_TIMEOUT; - /* start RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + rc_timeout) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && 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; -- cgit v1.2.3 From 3da219c3db638e0a57d18e892575df13d8c11f47 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 14:14:03 +0200 Subject: Update airspeed calibration routine to account for new signedness options --- src/modules/commander/airspeed_calibration.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 6039d92a7..c8c7a42e7 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 @@ -64,9 +64,9 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "don't move system"); + mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2500; + const int calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; @@ -85,13 +85,15 @@ int do_airspeed_calibration(int mavlink_fd) if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { + mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } close(fd); } if (!paramreset_successful) { - warn("WARNING: failed to set scale / offsets for airspeed sensor"); - mavlink_log_critical(mavlink_fd, "could not reset dpress sensor"); + warn("FAILED to set scale / offsets for airspeed"); + mavlink_log_critical(mavlink_fd, "dpress reset failed"); mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } @@ -107,7 +109,7 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; + diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) -- cgit v1.2.3 From 97cde3311efab479c43226dea3b1edd93629c33b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 5 Apr 2014 16:59:01 +0400 Subject: commander: home publication fixed --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 47e630d07..17c085652 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -613,10 +613,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* announce new home position */ if (*home_pub > 0) { - orb_publish(ORB_ID(home_position), *home_pub, &home); + orb_publish(ORB_ID(home_position), *home_pub, home); } else { - *home_pub = orb_advertise(ORB_ID(home_position), &home); + *home_pub = orb_advertise(ORB_ID(home_position), home); } /* mark home position as set */ -- cgit v1.2.3 From a4ba705e2f64616e25456c71572661f6b5e7cc3b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Apr 2014 20:04:18 +0400 Subject: commander: don't use mode switch if it's not mapped --- src/modules/commander/commander.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73e1a1792..aade0d862 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1514,6 +1514,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: + res = TRANSITION_NOT_CHANGED; + break; + case SWITCH_POS_OFF: // MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here -- cgit v1.2.3 From b770c9fc1edc570fc216bdf849f84519e4e3513f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:16:43 +0400 Subject: position_estimator_inav: increase acceptable EPH/EPV, in commander use EPH/EPV to decide if global position valid --- src/modules/commander/commander.cpp | 23 +++++++--- .../position_estimator_inav_main.c | 49 ++++++++++++++++------ 2 files changed, 53 insertions(+), 19 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17c085652..2f373c6db 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -153,6 +153,7 @@ static bool on_usb_power = false; static float takeoff_alt = 5.0f; static int parachute_enabled = 0; +static float eph_epv_threshold = 5.0f; static struct vehicle_status_s status; static struct actuator_armed_s armed; @@ -965,15 +966,25 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed); + /* hysteresis for EPH/EPV*/ + bool eph_epv_good; + if (status.condition_global_position_valid) { + if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { + eph_epv_good = false; + } + + } else { + if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { + eph_epv_good = true; + } + } + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ - static float hdop_threshold_m = 4.0f; - static float vdop_threshold_m = 8.0f; /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -1004,7 +1015,7 @@ 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.xy_valid && eph_epv_good, &(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); static bool published_condition_landed_fw = false; @@ -1322,7 +1333,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; 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 40f7069ca..dc85f7482 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -168,13 +168,13 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]); fwrite(s, 1, n, f); n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); fwrite(s, 1, n, f); @@ -199,6 +199,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; + memset(x_est_prev, 0, sizeof(x_est_prev)); + memset(y_est_prev, 0, sizeof(y_est_prev)); + memset(z_est_prev, 0, sizeof(z_est_prev)); + int baro_init_cnt = 0; int baro_init_num = 200; float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted @@ -249,6 +254,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; + static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation + static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation + float sonar_prev = 0.0f; hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) @@ -584,13 +592,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > 10.0f || gps.epv_m > 20.0f || gps.fix_type < 3) { + if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < 5.0f && gps.epv_m < 10.0f && gps.fix_type >= 3) { + if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -665,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } - w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); - w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } } else { @@ -782,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + } + /* inertial filter correction for altitude */ inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); - float x_est_prev[3], y_est_prev[3]; + if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + memcpy(z_est, z_est_prev, sizeof(z_est)); + memset(corr_acc, 0, sizeof(corr_acc)); + memset(corr_gps, 0, sizeof(corr_gps)); + corr_baro = 0; - memcpy(x_est_prev, x_est, sizeof(x_est)); - memcpy(y_est_prev, y_est, sizeof(y_est)); + } else { + memcpy(z_est_prev, z_est, sizeof(z_est)); + } if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); } @@ -822,13 +841,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { - write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); + if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) { + write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(y_est, y_est_prev, sizeof(y_est)); memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_flow, 0, sizeof(corr_flow)); + + } else { + memcpy(x_est_prev, x_est, sizeof(x_est)); + memcpy(y_est_prev, y_est, sizeof(y_est)); } } -- cgit v1.2.3 From dfd9601b571057e73668d9b39d584bc4eb9cc305 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 17:24:39 +0400 Subject: commander: minor comment fix --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2f373c6db..531d17145 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -966,7 +966,7 @@ int commander_thread_main(int argc, char *argv[]) } /* update condition_global_position_valid */ - /* hysteresis for EPH/EPV*/ + /* hysteresis for EPH/EPV */ bool eph_epv_good; if (status.condition_global_position_valid) { if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { -- cgit v1.2.3 From a4a12dab337f132bfe51db2ad482273ecfdf0ce6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:19 +0200 Subject: commander: put unsupported warning back in place --- src/modules/commander/commander.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 58995fcec..06f7ae6cc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -588,15 +588,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* silently ignore unsupported commands, maybe they are passed on over mavlink */ -// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -// /* already warned about unsupported commands in "default" case */ -// answer_command(*cmd, result); -// } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); + } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From c1545bd237cdaf9636da55f5b907bdadc39ed130 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Apr 2014 18:56:03 -0700 Subject: Fix float equality comparison Also restructured incorrect return statement --- src/modules/commander/commander.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1c11e2587..57faafcfd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -473,18 +473,19 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values - if (cmd->param1 != 0.0f && cmd->param1 != 1.0f) { - return VEHICLE_CMD_RESULT_UNSUPPORTED; - } - - transition_result_t arming_res = arm_disarm(cmd->param1 == 1.0f, mavlink_fd, "arm/disarm component command"); - if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. + // We use an float epsilon delta to test float equality. + if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } + transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + } } break; -- cgit v1.2.3 From a6215b7bda47616018d29b2f7de630deb4984be9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:06:52 +0200 Subject: commander: handle_command: filter commands by sysid and componentid --- src/modules/commander/commander.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 69a45a02f..89b8e684d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ -- cgit v1.2.3 From bccdbde45eb70271bd946ca86173cd8add20cd3d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:29:11 +0200 Subject: commander: handle_command: do not filter command if componentid == 0 --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89b8e684d..ce6de88ef 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe bool ret = false; /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } -- cgit v1.2.3 From 0b97dd2b776ce61fd53776f036230ea0089e26e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 9 Apr 2014 18:55:55 +0400 Subject: commander: brackets added to return switch check --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index aade0d862..d4567c4f1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1236,7 +1236,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE && + } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && pos_sp_triplet.nav_state == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ status.set_nav_state = NAV_STATE_MISSION; -- cgit v1.2.3 From 3fa82675e79013fedf9a787ca21e06e5bd15e5ca Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 3 Apr 2014 21:13:03 +0200 Subject: commander: don't beep if message is not understood --- src/modules/commander/commander.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d4567c4f1..27d5e4260 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -591,11 +591,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { - /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); - } + /* silently ignore unsupported commands, maybe they are passed on over mavlink */ +// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { +// /* already warned about unsupported commands in "default" case */ +// answer_command(*cmd, result); +// } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From 3dd64086e49192aabc020b50f48d68233bad392f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Apr 2014 21:42:19 +0200 Subject: commander: put unsupported warning back in place --- src/modules/commander/commander.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 27d5e4260..662b98b35 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -587,15 +587,15 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; default: - /* warn about unsupported commands */ + /* Warn about unsupported commands, this makes sense because only commands + * to this component ID (or all) are passed by mavlink. */ answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } - /* silently ignore unsupported commands, maybe they are passed on over mavlink */ -// if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -// /* already warned about unsupported commands in "default" case */ -// answer_command(*cmd, result); -// } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + /* already warned about unsupported commands in "default" case */ + answer_command(*cmd, result); + } /* send any requested ACKs */ if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { -- cgit v1.2.3 From 03a3b1d67127af8681275a50e5604ea1b60814ea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:06:52 +0200 Subject: commander: handle_command: filter commands by sysid and componentid --- src/modules/commander/commander.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 662b98b35..e6de58563 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -395,6 +395,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; bool ret = false; + /* only handle commands that are meant to be handled by this system and component */ + if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + return false; + } + /* only handle high-priority commands here */ /* request to set different system mode */ -- cgit v1.2.3 From 93227f9200ee3369f0d2a3aa2fa7bb2a738e18c9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Apr 2014 17:29:11 +0200 Subject: commander: handle_command: do not filter command if componentid == 0 --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e6de58563..e7cf2b3fa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -396,7 +396,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe bool ret = false; /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || cmd->target_component != status->component_id) { + if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } -- cgit v1.2.3 From 46a796fb86986c5172ab4d85d1902e7648afd651 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef..7ac7aff0f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); -- cgit v1.2.3 From 37b133e231e945c978dd66fd5daed0f12caa8073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef..7ac7aff0f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); -- cgit v1.2.3 From c3c0328e8bb9211580dbe5a52ecb23e0452cb402 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 21 Apr 2014 17:36:59 +0200 Subject: navigator: lot's of cleanup (WIP) --- src/modules/commander/commander.cpp | 57 +- src/modules/commander/state_machine_helper.cpp | 6 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 8 +- src/modules/mavlink/mavlink_main.cpp | 7 +- src/modules/mavlink/mavlink_messages.cpp | 5 +- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 +- src/modules/navigator/geofence_params.c | 1 - src/modules/navigator/mission.cpp | 297 +++++++ src/modules/navigator/mission.h | 98 +++ src/modules/navigator/module.mk | 6 +- src/modules/navigator/navigator_main.cpp | 934 +++++++-------------- src/modules/navigator/navigator_mission.cpp | 332 -------- src/modules/navigator/navigator_mission.h | 106 --- src/modules/navigator/navigator_params.c | 41 - src/modules/navigator/navigator_state.h | 21 - src/modules/navigator/rtl.cpp | 253 ++++++ src/modules/navigator/rtl.h | 92 ++ src/modules/navigator/rtl_params.c | 87 ++ src/modules/systemlib/state_table.h | 23 +- src/modules/uORB/topics/mission.h | 2 + .../uORB/topics/position_setpoint_triplet.h | 12 +- src/modules/uORB/topics/vehicle_global_position.h | 12 +- src/modules/uORB/topics/vehicle_status.h | 13 +- 23 files changed, 1221 insertions(+), 1194 deletions(-) create mode 100644 src/modules/navigator/mission.cpp create mode 100644 src/modules/navigator/mission.h delete mode 100644 src/modules/navigator/navigator_mission.cpp delete mode 100644 src/modules/navigator/navigator_mission.h delete mode 100644 src/modules/navigator/navigator_state.h create mode 100644 src/modules/navigator/rtl.cpp create mode 100644 src/modules/navigator/rtl.h create mode 100644 src/modules/navigator/rtl_params.c (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fa..c2d24a883 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -547,15 +547,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAV_STATE_LOITER; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAV_STATE_MISSION; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; ret = true; @@ -668,8 +666,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; - status.set_nav_state_timestamp = 0; + status.set_nav_state = NAVIGATION_STATE_NONE; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -944,7 +941,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; - if (status.is_rotary_wing && status.condition_local_altitude_valid) { + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; @@ -1222,30 +1219,30 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); + if (sp_man.mode_switch == SWITCH_POS_ON) { - } else { - /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAV_STATE_LOITER; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); + /* set navigation state */ + /* RETURN switch, overrides MISSION switch */ + if (sp_man.return_switch == SWITCH_POS_ON) { + /* switch to RTL if not already landed after RTL and home position set */ + status.set_nav_state = NAVIGATION_STATE_RTL; + + } else { + /* MISSION switch */ + if (sp_man.mission_switch == SWITCH_POS_ON) { + /* stick is in LOITER position */ + status.set_nav_state = NAVIGATION_STATE_LOITER; + + } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + /* stick is in MISSION position */ + status.set_nav_state = NAVIGATION_STATE_MISSION; + } + /* XXX: I don't understand this */ + //else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && + // pos_sp_triplet.nav_state == NAVIGATION_STATE_RTL) { + // /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ + // status.set_nav_state = NAV_STATE_MISSION; + // } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e6f245cf9..54f0f13f4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -450,8 +450,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAV_STATE_RTL; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_RTL; ret = TRANSITION_CHANGED; } @@ -461,8 +460,7 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f /* at least relative altitude estimate required for landing */ if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAV_STATE_LAND; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->set_nav_state = NAVIGATION_STATE_LAND; ret = TRANSITION_CHANGED; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df785..a00a388a8 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -150,8 +150,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - /** manual control states */ float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ double _loiter_hold_lat; @@ -393,7 +391,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), /* states */ - _setpoint_valid(false), _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), @@ -663,7 +660,6 @@ FixedwingPositionControl::vehicle_setpoint_poll() if (pos_sp_triplet_updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - _setpoint_valid = true; } } @@ -708,7 +704,7 @@ void FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); @@ -781,7 +777,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. - if (_control_mode.flag_control_position_enabled) { + if (pos_sp_triplet.current.valid) { /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 36d47b7ee..c60c85495 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -872,7 +872,7 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item break; case MAV_CMD_DO_JUMP: mission_item->do_jump_mission_index = mavlink_mission_item->param1; - /* TODO: also save param2 (repeat count) */ + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; @@ -889,6 +889,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + return OK; } @@ -908,7 +911,7 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ case NAV_CMD_DO_JUMP: mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - /* TODO: also save param2 (repeat count) */ + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 648228e3b..3cf69bf7c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -117,7 +117,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { + if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND + || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { /* use main state when navigator is not active */ if (status->main_state == MAIN_STATE_MANUAL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); @@ -142,7 +143,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { + if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2bd2d356a..7d3d39d18 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -645,7 +645,7 @@ MulticopterPositionControl::task_main() _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = true; - _pos_sp_triplet.nav_state = NAV_STATE_NONE; + // _pos_sp_triplet.nav_state = NAV_STATE_NONE; _pos_sp_triplet.current.type = SETPOINT_TYPE_NORMAL; _pos_sp_triplet.current.lat = _lat_sp; _pos_sp_triplet.current.lon = _lon_sp; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 5831a0ca9..653b1ad84 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp new file mode 100644 index 000000000..e95bcfd43 --- /dev/null +++ b/src/modules/navigator/mission.cpp @@ -0,0 +1,297 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator_mission.cpp + * + * Helper class to access missions + * + * @author Julian Oes + */ + +#include +#include + +#include +#include + +#include +#include + +#include "mission.h" + + +Mission::Mission() : + + _offboard_dataman_id(-1), + _current_offboard_mission_index(0), + _current_onboard_mission_index(0), + _offboard_mission_item_count(0), + _onboard_mission_item_count(0), + _onboard_mission_allowed(false), + _current_mission_type(MISSION_TYPE_NONE), + _mission_result_pub(-1), + _mission_result({}) +{ +} + +Mission::~Mission() +{ +} + +void +Mission::set_offboard_dataman_id(const int new_id) +{ + _offboard_dataman_id = new_id; +} + +void +Mission::set_offboard_mission_count(int new_count) +{ + _offboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_count(int new_count) +{ + _onboard_mission_item_count = new_count; +} + +void +Mission::set_onboard_mission_allowed(bool allowed) +{ + _onboard_mission_allowed = allowed; +} + +bool +Mission::command_current_offboard_mission_index(const int new_index) +{ + if (new_index >= 0) { + _current_offboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= _offboard_mission_item_count) { + _current_offboard_mission_index = 0; + } + } + report_current_offboard_mission_item(); +} + +bool +Mission::command_current_onboard_mission_index(int new_index) +{ + if (new_index != -1) { + _current_onboard_mission_index = (unsigned)new_index; + } else { + + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= _onboard_mission_item_count) { + _current_onboard_mission_index = 0; + } + } + // TODO: implement this for onboard missions as well + // report_current_mission_item(); +} + +bool +Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +{ + *onboard = false; + *index = -1; + + /* try onboard mission first */ + if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { + _current_mission_type = MISSION_TYPE_ONBOARD; + *onboard = true; + *index = _current_onboard_mission_index; + + return true; + } + } + + /* otherwise fallback to offboard */ + if (_current_offboard_mission_index < _offboard_mission_item_count) { + + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { + + _current_mission_type = MISSION_TYPE_OFFBOARD; + *onboard = false; + *index = _current_offboard_mission_index; + + return true; + } + } + + /* happens when no more mission items can be added as a next item */ + _current_mission_type = MISSION_TYPE_NONE; + + return false; +} + +bool +Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +{ + int next_temp_mission_index = _current_onboard_mission_index + 1; + + /* try onboard mission first */ + if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* then try offboard mission */ + dm_item_t dm_current; + if (_offboard_dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + next_temp_mission_index = _current_offboard_mission_index + 1; + if (next_temp_mission_index < _offboard_mission_item_count) { + if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { + return true; + } + } + + /* both failed, bail out */ + return false; +} + +bool +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ + if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { + return false; + } + + } else { + /* if it's not a DO_JUMP, then we were successful */ + return true; + } + } + + /* we have given up, we don't want to cycle forever */ + warnx("ERROR: cycling through mission items without success"); + return false; +} + +void +Mission::move_to_next() +{ + report_mission_item_reached(); + + switch (_current_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } +} + +void +Mission::report_mission_item_reached() +{ + if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } + publish_mission_result(); +} + +void +Mission::report_current_offboard_mission_item() +{ + _mission_result.index_current_mission = _current_offboard_mission_index; + publish_mission_result(); +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.mission_reached = false; +} + diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h new file mode 100644 index 000000000..0fa2ff3fa --- /dev/null +++ b/src/modules/navigator/mission.h @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator_mission.h + * Helper class to access missions + * @author Julian Oes + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include +#include +#include + + +class __EXPORT Mission +{ +public: + /** + * Constructor + */ + Mission(); + + /** + * Destructor + */ + ~Mission(); + + void set_offboard_dataman_id(const int new_id); + void set_offboard_mission_count(const int new_count); + void set_onboard_mission_count(const int new_count); + void set_onboard_mission_allowed(const bool allowed); + + bool command_current_offboard_mission_index(const int new_index); + bool command_current_onboard_mission_index(const int new_index); + + bool get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, int *index); + bool get_next_mission_item(struct mission_item_s *mission_item); + + void move_to_next(); + +private: + bool read_mission_item(const dm_item_t dm_item, const bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + + void report_mission_item_reached(); + void report_current_offboard_mission_item(); + + void publish_mission_result(); + + int _offboard_dataman_id; + int _current_offboard_mission_index; + int _current_onboard_mission_index; + int _offboard_mission_item_count; /** number of offboard mission items available */ + int _onboard_mission_item_count; /** number of onboard mission items available */ + bool _onboard_mission_allowed; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD, + } _current_mission_type; + + orb_advert_t _mission_result_pub; /**< publish mission result topic */ + mission_result_s _mission_result; /**< mission result for commander/mavlink */ +}; + +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 55f8a4caa..5f113f686 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 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 @@ -39,7 +39,9 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp \ + mission.cpp \ + rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 494266dd3..34a28aec3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Jean Cyr - * @author Julian Oes - * @author Anton Babushkin + * Copyright (c) 2013-2014 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 @@ -35,19 +31,20 @@ * ****************************************************************************/ /** - * @file navigator_main.c + * @file navigator_main.cpp * Implementation of the main navigation state machine. * - * Handles missions, geo fencing and failsafe navigation behavior. - * Published the mission item triplet for the position controller. + * Handles mission items, geo fencing and failsafe navigation behavior. + * Published the position setpoint triplet for the position controller. * * @author Lorenz Meier * @author Jean Cyr - * @author Julian Oes + * @author Julian Oes * @author Anton Babushkin */ #include + #include #include #include @@ -58,9 +55,13 @@ #include #include #include +#include +#include + #include #include #include + #include #include #include @@ -71,20 +72,19 @@ #include #include #include + #include #include #include #include #include #include -#include #include +#include #include -#include -#include -#include "navigator_state.h" -#include "navigator_mission.h" +#include "mission.h" +#include "rtl.h" #include "mission_feasibility_checker.h" #include "geofence.h" @@ -130,12 +130,12 @@ public: /** * Add point to geofence */ - void add_fence_point(int argc, char *argv[]); + void add_fence_point(int argc, char *argv[]); /** * Load fence from file */ - void load_fence_from_file(const char *filename); + void load_fence_from_file(const char *filename); private: @@ -154,15 +154,15 @@ private: int _control_mode_sub; /**< vehicle control mode subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_item_s _mission_item; /**< current mission item */ + + bool _mission_item_valid; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -174,56 +174,41 @@ private: struct navigation_capabilities_s _nav_caps; - class Mission _mission; + Mission _mission; + + RTL _rtl; - bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position.global_valid flag */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ - MissionFeasibilityChecker missionFeasiblityChecker; - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - - bool _pos_sp_triplet_updated; - - const char *nav_states_str[NAV_STATE_MAX]; + bool _update_triplet; struct { - float min_altitude; float acceptance_radius; float loiter_radius; int onboard_mission_enabled; float takeoff_alt; - float land_alt; - float rtl_alt; - float rtl_land_delay; } _parameters; /**< local copies of parameters */ struct { - param_t min_altitude; param_t acceptance_radius; param_t loiter_radius; param_t onboard_mission_enabled; param_t takeoff_alt; - param_t land_alt; - param_t rtl_alt; - param_t rtl_land_delay; } _parameter_handles; /**< handles for parameters */ enum Event { EVENT_NONE_REQUESTED, - EVENT_READY_REQUESTED, EVENT_LOITER_REQUESTED, EVENT_MISSION_REQUESTED, EVENT_RTL_REQUESTED, - EVENT_LAND_REQUESTED, + EVENT_TAKEN_OFF, + EVENT_LANDED, EVENT_MISSION_CHANGED, EVENT_HOME_POSITION_CHANGED, + EVENT_MISSION_ITEM_REACHED, MAX_EVENT }; @@ -232,15 +217,6 @@ private: */ static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND - }; - - enum RTLState _rtl_state; - /** * Update our local parameter cache. */ @@ -264,7 +240,7 @@ private: /** * Retrieve offboard mission. */ - void offboard_mission_update(bool isrotaryWing); + void offboard_mission_update(); /** * Retrieve onboard mission. @@ -296,19 +272,20 @@ private: /** * Functions that are triggered when a new state is entered. */ - void start_none(); - void start_ready(); - void start_loiter(); - void start_mission(); - void start_rtl(); - void start_land(); - void start_land_home(); + bool start_none_on_ground(); + bool start_none_in_air(); + bool start_auto_on_ground(); + bool start_loiter(); + bool start_mission(); + bool advance_mission(); + bool start_rtl(); + bool advance_rtl(); + bool start_land(); /** * Fork for state transitions */ - void request_loiter_or_ready(); - void request_mission_if_available(); + // void request_loiter_or_ready(); /** * Guards offboard mission @@ -333,12 +310,12 @@ private: /** * Perform actions when current mission item reached. */ - void on_mission_item_reached(); + // void on_mission_item_reached(); /** * Move to next waypoint */ - void set_mission_item(); + bool set_mission_items(); /** * Switch to next RTL state @@ -348,7 +325,7 @@ private: /** * Set position setpoint for mission item */ - void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); /** * Helper function to get a takeoff item @@ -377,12 +354,9 @@ Navigator::Navigator() : /* state machine transition table */ StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), - _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), - -/* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), @@ -391,53 +365,29 @@ Navigator::Navigator() : _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - -/* publications */ _pos_sp_triplet_pub(-1), - -/* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), _mission(), + _pos_sp_triplet({}), + _mission_item({}), _mission_item_valid(false), - _global_pos_valid(false), - _reset_loiter_pos(true), _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _need_takeoff(true), - _do_takeoff(false), - _set_nav_state_timestamp(0), - _pos_sp_triplet_updated(false), -/* states */ - _rtl_state(RTL_STATE_NONE) + _update_triplet(false) { - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); - _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - - memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_item, 0, sizeof(struct mission_item_s)); - - memset(&nav_states_str, 0, sizeof(nav_states_str)); - nav_states_str[0] = "NONE"; - nav_states_str[1] = "READY"; - nav_states_str[2] = "LOITER"; - nav_states_str[3] = "MISSION"; - nav_states_str[4] = "RTL"; - nav_states_str[5] = "LAND"; /* Initialize state machine */ - myState = NAV_STATE_NONE; - start_none(); + myState = NAV_STATE_NONE_ON_GROUND; + + start_none_on_ground(); } Navigator::~Navigator() @@ -472,16 +422,12 @@ Navigator::parameters_update() struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); - param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); - param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); - param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); + _mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled); _geofence.updateParams(); } @@ -496,6 +442,8 @@ void Navigator::home_position_update() { orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos); + + _rtl.set_home_position(&_home_pos); } void @@ -506,7 +454,7 @@ Navigator::navigation_capabilities_update() void -Navigator::offboard_mission_update(bool isrotaryWing) +Navigator::offboard_mission_update() { struct mission_s offboard_mission; @@ -523,19 +471,17 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(_vstatus.is_rotary_wing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_offboard_mission_count(offboard_mission.count); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); + _mission.command_current_offboard_mission_index(offboard_mission.current_index); } else { _mission.set_offboard_mission_count(0); - _mission.set_current_offboard_mission_index(0); + _mission.command_current_offboard_mission_index(0); } - - _mission.publish_mission_result(); } void @@ -546,11 +492,11 @@ Navigator::onboard_mission_update() if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { _mission.set_onboard_mission_count(onboard_mission.count); - _mission.set_current_onboard_mission_index(onboard_mission.current_index); + _mission.command_current_onboard_mission_index(onboard_mission.current_index); } else { _mission.set_onboard_mission_count(0); - _mission.set_current_onboard_mission_index(0); + _mission.command_current_onboard_mission_index(0); } } @@ -626,13 +572,13 @@ Navigator::task_main() global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(_vstatus.is_rotary_wing); + offboard_mission_update(); onboard_mission_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; + unsigned prevState = NAV_STATE_NONE_ON_GROUND; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; @@ -690,55 +636,45 @@ Navigator::task_main() if (fds[6].revents & POLLIN) { vehicle_status_update(); - /* evaluate state requested by commander */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } - } - - } else { - /* navigator shouldn't act */ + /* commander requested new navigation mode, forward it to state machine */ + switch (_vstatus.set_nav_state) { + case NAVIGATION_STATE_NONE: dispatch(EVENT_NONE_REQUESTED); + break; + + case NAVIGATION_STATE_LOITER: + dispatch(EVENT_LOITER_REQUESTED); + break; + + case NAVIGATION_STATE_MISSION: + dispatch(EVENT_MISSION_REQUESTED); + break; + + case NAVIGATION_STATE_RTL: + /* TODO: what is this for? */ + // if (!(_rtl_state == RTL_STATE_DESCEND && + // (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && + // _vstatus.condition_home_position_valid) { + dispatch(EVENT_RTL_REQUESTED); + // } + break; + + case NAVIGATION_STATE_LAND: + /* TODO: add this */ + // dispatch(EVENT_LAND_REQUESTED); + break; + + default: + warnx("ERROR: Requested navigation state not supported"); + break; } - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; + /* commander sets in-air/on-ground flag as well */ + if (_vstatus.condition_landed) { + dispatch(EVENT_LANDED); + } else { + dispatch(EVENT_TAKEN_OFF); + } } /* parameters updated */ @@ -754,74 +690,49 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { - offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed + offboard_mission_update(); + /* TODO: check if mission really changed */ dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); - // XXX check if mission really changed + /* TODO: check if mission really changed */ dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); - // XXX check if home position really changed + /* TODO check if home position really changed */ dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - - /* publish position setpoint triplet on each position update if navigator active */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - _pos_sp_triplet_updated = true; - - if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) { - /* got global position when landing, update setpoint */ - start_land(); - } - - _global_pos_valid = _global_pos.global_valid; - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } + if (check_mission_item_reached()) { + dispatch(EVENT_MISSION_ITEM_REACHED); } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { - //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } - } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } - /* publish position setpoint triplet if updated */ - if (_pos_sp_triplet_updated) { - _pos_sp_triplet_updated = false; + if (_update_triplet ) { publish_position_setpoint_triplet(); - } - - /* notify user about state changes */ - if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); - prevState = myState; + _update_triplet = false; } perf_end(_loop_perf); @@ -879,8 +790,12 @@ Navigator::status() } switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); + case NAV_STATE_NONE_ON_GROUND: + warnx("State: None on ground"); + break; + + case NAV_STATE_NONE_IN_AIR: + warnx("State: None in air"); break; case NAV_STATE_LOITER: @@ -895,6 +810,10 @@ Navigator::status() warnx("State: RTL"); break; + case NAV_STATE_LAND: + warnx("State: LAND"); + break; + default: warnx("State: Unknown"); break; @@ -903,92 +822,119 @@ Navigator::status() StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { - /* NAV_STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* NAV_STATE_NONE_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, + }, + { + /* NAV_STATE_NONE_IN_AIR */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, }, { - /* NAV_STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, + /* NAV_STATE_AUTO_ON_GROUND */ + /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, }, { /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, }, { /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, }, { /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, + /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state + /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, + /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, }, { /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, + /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, + /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, }, }; -void -Navigator::start_none() +bool +Navigator::start_none_on_ground() { reset_reached(); _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = false; _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - _reset_loiter_pos = true; - _do_takeoff = false; - _rtl_state = RTL_STATE_NONE; + _update_triplet = true; + return true; +} + +bool +Navigator::start_none_in_air() +{ + reset_reached(); + + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void -Navigator::start_ready() +bool +Navigator::start_auto_on_ground() { reset_reached(); @@ -998,46 +944,26 @@ Navigator::start_ready() _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - if (_rtl_state != RTL_STATE_DESCEND) { - /* reset RTL state if landed not at home */ - _rtl_state = RTL_STATE_NONE; - } + // if (_rtl_state != RTL_STATE_DESCEND) { + // reset RTL state if landed not at home + // _rtl_state = RTL_STATE_NONE; + // } - _pos_sp_triplet_updated = true; + _update_triplet = true; + return true; } -void +bool Navigator::start_loiter() { - reset_reached(); - - _do_takeoff = false; - - /* set loiter position if needed */ - if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { - _reset_loiter_pos = false; + /* if no existing item available, use current position */ + if (!(_pos_sp_triplet.current.valid && _waypoint_position_reached)) { _pos_sp_triplet.current.lat = _global_pos.lat; _pos_sp_triplet.current.lon = _global_pos.lon; _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - - float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; - - /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { - _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - - } else { - _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - } - + _pos_sp_triplet.current.alt = _global_pos.alt; } _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; @@ -1045,167 +971,146 @@ Navigator::start_loiter() _pos_sp_triplet.previous.valid = false; _pos_sp_triplet.current.valid = true; _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - _pos_sp_triplet_updated = true; + mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); + + _update_triplet = true; + return true; } -void +bool Navigator::start_mission() { - _need_takeoff = true; + /* start fresh */ + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; - set_mission_item(); + return set_mission_items(); } -void -Navigator::set_mission_item() +bool +Navigator::advance_mission() { - reset_reached(); + /* tell mission to move by one */ + _mission.move_to_next(); - /* copy current mission to previous item */ - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + /* now try to set the new mission items, if it fails, it will dispatch loiter */ + return set_mission_items(); +} - _reset_loiter_pos = true; - _do_takeoff = false; +bool +Navigator::set_mission_items() +{ + if (_pos_sp_triplet.current.valid) { + memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + _pos_sp_triplet.previous.valid = true; + } - int ret; bool onboard; - unsigned index; + int index; - ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); + /* if we fail to set the current mission, continue to loiter */ + if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - if (ret == OK) { - _mission.report_current_offboard_mission_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; + } - _mission_item_valid = true; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { - /* don't reset RTL state on RTL or LOITER items */ - _rtl_state = RTL_STATE_NONE; - } + /* if we got an RTL mission item, switch to RTL mode and give up */ + if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + dispatch(EVENT_RTL_REQUESTED); + return false; + } - if (_vstatus.is_rotary_wing) { - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { - /* do special TAKEOFF handling for VTOL */ - _need_takeoff = false; - - /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _pos_sp_triplet.current.alt; - - if (_vstatus.condition_landed) { - /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ - takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); - } + _mission_item_valid = true; - /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { - /* force TAKEOFF if landed or waypoint altitude is more than current */ - _do_takeoff = true; + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; + - /* move current position setpoint to next */ - memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); + mission_item_s next_mission_item; - /* set current setpoint to takeoff */ + bool last_wp = false; + /* now try to set the next mission item as well, if there is no more next + * this means we're heading to the last waypoint */ + if (_mission.get_next_mission_item(&next_mission_item)) { + /* convert the next mission item and set it valid */ + mission_item_to_position_setpoint(&next_mission_item, &_pos_sp_triplet.next); + _pos_sp_triplet.next.valid = true; + } else { + last_wp = true; + } - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = takeoff_alt_amsl; - _pos_sp_triplet.current.yaw = NAN; - _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; - } + /* notify user about what happened */ + mavlink_log_info(_mavlink_fd, "#audio: heading to %s%swaypoint %d", + (last_wp ? "last " : "" ), (onboard ? "onboard " : ""), index); - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - /* will need takeoff after landing */ - _need_takeoff = true; - } - } + _update_triplet = true; - if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); + reset_reached(); - } else { - if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); + return true; +} - } else { - mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); - } - } +bool +Navigator::start_rtl() +{ + /* TODO check if RTL was successfull, if not we have a problem and need to dispatch something */ + if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - } else { - /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_valid = false; - _pos_sp_triplet.current.valid = false; - warnx("ERROR: current WP can't be set"); - } + _mission_item_valid = true; - if (!_do_takeoff) { - mission_item_s item_next; - ret = _mission.get_next_mission_item(&item_next); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; - if (ret == OK) { - position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); + reset_reached(); - } else { - /* this will fail for the last WP */ - _pos_sp_triplet.next.valid = false; - } + _update_triplet = true; + return true; } + dispatch(EVENT_LOITER_REQUESTED); - _pos_sp_triplet_updated = true; + return false; } -void -Navigator::start_rtl() +bool +Navigator::advance_rtl() { - _do_takeoff = false; + /* tell mission to move by one */ + _rtl.move_to_next(); - /* decide if we need climb */ - if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { - _rtl_state = RTL_STATE_CLIMB; + /* now try to set the new mission items, if it fails, it will dispatch loiter */ + if (_rtl.get_current_rtl_item(&_global_pos, &_mission_item)) { - } else { - _rtl_state = RTL_STATE_RETURN; - } - } + _mission_item_valid = true; - /* if switching directly to return state, reset altitude setpoint */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); + _pos_sp_triplet.current.valid = true; + + reset_reached(); + + _update_triplet = true; + return true; } - _reset_loiter_pos = true; - set_rtl_item(); + dispatch(EVENT_LOITER_REQUESTED); + return false; } -void +bool Navigator::start_land() { + /* TODO: verify/test */ + reset_reached(); /* this state can be requested by commander even if no global position available, * in his case controller must perform landing without position control */ - _do_takeoff = false; - _reset_loiter_pos = true; memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _global_pos.lat; _mission_item.lon = _global_pos.lon; _mission_item.altitude_is_relative = false; @@ -1220,185 +1125,22 @@ Navigator::start_land() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::start_land_home() -{ - reset_reached(); - - /* land to home position, should be called when hovering above home, from RTL state */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - _mission_item_valid = true; - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); + mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current); _pos_sp_triplet.next.valid = false; -} - -void -Navigator::set_rtl_item() -{ - reset_reached(); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - } - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); - break; - } - - case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // don't change altitude - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - default: { - mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::request_loiter_or_ready() -{ - /* XXX workaround: no landing detector for fixedwing yet */ - if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } -} - -void -Navigator::request_mission_if_available() -{ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - } else { - request_loiter_or_ready(); - } + _update_triplet = true; + return true; } void -Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) +Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { sp->valid = true; if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* set home position for RTL item */ - sp->lat = _home_pos.lat; - sp->lon = _home_pos.lon; - sp->alt = _home_pos.alt + _parameters.rtl_alt; if (_pos_sp_triplet.previous.valid) { /* if previous setpoint is valid then use it to calculate heading to home */ @@ -1408,9 +1150,6 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_ /* else use current position */ sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); } - sp->loiter_radius = _parameters.loiter_radius; - sp->loiter_direction = 1; - sp->pitch_min = 0.0f; } else { sp->lat = item->lat; @@ -1450,14 +1189,14 @@ Navigator::check_mission_item_reached() return _vstatus.condition_landed; } - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { + /* TODO count turns */ + // if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + // _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + // _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + // _mission_item.loiter_radius > 0.01f) { - return false; - } + // return false; + // } uint64_t now = hrt_absolute_time(); @@ -1475,22 +1214,19 @@ Navigator::check_mission_item_reached() float dist_xy = -1.0f; float dist_z = -1.0f; - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _home_pos.alt : _mission_item.altitude; - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _global_pos.lat, _global_pos.lon, _global_pos.alt, &dist_xy, &dist_z); - if (_do_takeoff) { - if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { - /* require only altitude for takeoff */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_global_pos.alt > altitude_amsl - acceptance_radius) { _waypoint_position_reached = true; } - } else { if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; @@ -1499,7 +1235,10 @@ Navigator::check_mission_item_reached() } if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { + + /* TODO: removed takeoff, why? */ + if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) { + /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); @@ -1514,24 +1253,23 @@ Navigator::check_mission_item_reached() /* check if the current waypoint was reached */ if (_waypoint_position_reached && _waypoint_yaw_reached) { + if (_time_first_inside_orbit == 0) { _time_first_inside_orbit = now; if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); + mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + (double)_mission_item.time_inside); } } /* check if the MAV was long enough inside the waypoint orbit */ if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - reset_reached(); return true; } } - return false; - } void @@ -1543,62 +1281,6 @@ Navigator::reset_reached() } -void -Navigator::on_mission_item_reached() -{ - if (myState == NAV_STATE_MISSION) { - - _mission.report_mission_item_reached(); - - if (_do_takeoff) { - /* takeoff completed */ - _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); - - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - if (_mission.current_mission_available()) { - set_mission_item(); - - } else { - /* if no more mission items available then finish mission */ - /* loiter at last waypoint */ - _reset_loiter_pos = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - request_loiter_or_ready(); - } - - } else if (myState == NAV_STATE_RTL) { - /* RTL completed */ - if (_rtl_state == RTL_STATE_DESCEND) { - /* hovering above home position, land if needed or loiter */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - - if (_mission_item.autocontinue) { - dispatch(EVENT_LAND_REQUESTED); - - } else { - _reset_loiter_pos = false; - dispatch(EVENT_LOITER_REQUESTED); - } - - } else { - /* next RTL step */ - _rtl_state = (RTLState)(_rtl_state + 1); - set_rtl_item(); - } - - } else if (myState == NAV_STATE_LAND) { - /* landing completed */ - mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); - dispatch(EVENT_READY_REQUESTED); - } - _mission.publish_mission_result(); -} - void Navigator::publish_position_setpoint_triplet() { @@ -1607,11 +1289,9 @@ Navigator::publish_position_setpoint_triplet() /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { - /* advertise and publish */ _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp deleted file mode 100644 index ac7e604ef..000000000 --- a/src/modules/navigator/navigator_mission.cpp +++ /dev/null @@ -1,332 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author 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 navigator_mission.cpp - * Helper class to access missions - */ - -#include -#include -#include -#include -#include -#include -#include "navigator_mission.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - - -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), - _mission_result_pub(-1) -{ - memset(&_mission_result, 0, sizeof(struct mission_result_s)); -} - -Mission::~Mission() -{ - -} - -void -Mission::set_offboard_dataman_id(int new_id) -{ - _offboard_dataman_id = new_id; -} - -void -Mission::set_current_offboard_mission_index(int new_index) -{ - if (new_index != -1) { - warnx("specifically set to %d", new_index); - _current_offboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } - } - report_current_offboard_mission_item(); -} - -void -Mission::set_current_onboard_mission_index(int new_index) -{ - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } - } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); -} - -void -Mission::set_offboard_mission_count(unsigned new_count) -{ - _offboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_count(unsigned new_count) -{ - _onboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; -} - -bool -Mission::current_mission_available() -{ - return (current_onboard_mission_available() || current_offboard_mission_available()); - -} - -bool -Mission::next_mission_available() -{ - return (next_onboard_mission_available() || next_offboard_mission_available()); -} - -int -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (current_onboard_mission_available()) { - - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &_current_onboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - } - - /* otherwise fallback to offboard */ - } else if (current_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - ret = get_mission_item(dm_current, &_current_offboard_mission_index, new_mission_item); - if (ret == OK) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - } - - } else { - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; - ret == ERROR; - } - - return ret; -} - -int -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) -{ - int ret = ERROR; - - /* try onboard mission first */ - if (next_onboard_mission_available()) { - - unsigned next_onboard_mission_index = _current_onboard_mission_index + 1; - ret = get_mission_item(DM_KEY_WAYPOINTS_ONBOARD, &next_onboard_mission_index, new_mission_item); - - /* otherwise fallback to offboard */ - } else if (next_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - unsigned next_offboard_mission_index = _current_offboard_mission_index + 1; - ret = get_mission_item(dm_current, &next_offboard_mission_index, new_mission_item); - - } else { - /* happens when no more mission items can be added as a next item */ - ret = ERROR; - } - return ret; -} - - -bool -Mission::current_onboard_mission_available() -{ - return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; -} - -bool -Mission::current_offboard_mission_available() -{ - return _offboard_mission_item_count > _current_offboard_mission_index; -} - -bool -Mission::next_onboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_ONBOARD) { - next = 1; - } - - return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; -} - -bool -Mission::next_offboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_OFFBOARD) { - next = 1; - } - - return _offboard_mission_item_count > (_current_offboard_mission_index + next); -} - -void -Mission::move_to_next() -{ - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::report_mission_item_reached() -{ - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; - } -} - -void -Mission::report_current_offboard_mission_item() -{ - _mission_result.index_current_mission = _current_offboard_mission_index; -} - -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.mission_reached = false; -} - -int -Mission::get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item) -{ - /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ - for (int i=0; i<10; i++) { - - const ssize_t len = sizeof(struct mission_item_s); - - /* read mission item from datamanager */ - if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* check for DO_JUMP item */ - if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; - } else { - /* if it's not a DO_JUMP, then we were successful */ - return OK; - } - } - - /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); - return ERROR; -} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/navigator_mission.h deleted file mode 100644 index fef145410..000000000 --- a/src/modules/navigator/navigator_mission.h +++ /dev/null @@ -1,106 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: @author 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 navigator_mission.h - * Helper class to access missions - */ - -#ifndef NAVIGATOR_MISSION_H -#define NAVIGATOR_MISSION_H - -#include -#include - - -class __EXPORT Mission -{ -public: - /** - * Constructor - */ - Mission(); - - /** - * Destructor, also kills the sensors task. - */ - ~Mission(); - - void set_offboard_dataman_id(int new_id); - void set_current_offboard_mission_index(int new_index); - void set_current_onboard_mission_index(int new_index); - void set_offboard_mission_count(unsigned new_count); - void set_onboard_mission_count(unsigned new_count); - - void set_onboard_mission_allowed(bool allowed); - - bool current_mission_available(); - bool next_mission_available(); - - int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); - int get_next_mission_item(struct mission_item_s *mission_item); - - void move_to_next(); - - void report_mission_item_reached(); - void report_current_offboard_mission_item(); - void publish_mission_result(); - -private: - int get_mission_item(const dm_item_t dm_item, unsigned *mission_index, struct mission_item_s *new_mission_item); - - bool current_onboard_mission_available(); - bool current_offboard_mission_available(); - bool next_onboard_mission_available(); - bool next_offboard_mission_available(); - - int _offboard_dataman_id; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items available */ - unsigned _onboard_mission_item_count; /** number of onboard mission items available */ - - bool _onboard_mission_allowed; - - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; - - int _mission_result_pub; - - struct mission_result_s _mission_result; -}; - -#endif diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 9ef359c6d..49969a382 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -52,16 +52,6 @@ * Navigator parameters, accessible via MAVLink */ -/** - * Minimum altitude (fixed wing only) - * - * Minimum altitude above home for LOITER. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); - /** * Waypoint acceptance radius * @@ -101,37 +91,6 @@ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); -/** - * Landing altitude - * - * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); - -/** - * Return-To-Launch altitude - * - * Minimum altitude above home position for going home in RTL mode. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); - -/** - * Return-To-Launch delay - * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. - * - * @unit seconds - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); - /** * Enable parachute deployment * diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/navigator_state.h deleted file mode 100644 index 6a1475c9b..000000000 --- a/src/modules/navigator/navigator_state.h +++ /dev/null @@ -1,21 +0,0 @@ -/* - * navigator_state.h - * - * Created on: 27.01.2014 - * Author: ton - */ - -#ifndef NAVIGATOR_STATE_H_ -#define NAVIGATOR_STATE_H_ - -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; - -#endif /* NAVIGATOR_STATE_H_ */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp new file mode 100644 index 000000000..6e621e39d --- /dev/null +++ b/src/modules/navigator/rtl.cpp @@ -0,0 +1,253 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator_rtl.cpp + * Helper class to access RTL + * @author Julian Oes + * @author Anton Babushkin + */ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include "rtl.h" + + +RTL::RTL() : + SuperBlock(NULL, "RTL"), + _mavlink_fd(-1), + _rtl_state(RTL_STATE_NONE), + _home_position({}), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY"), + _loiter_radius(50), + _acceptance_radius(50) +{ + /* load initial params */ + updateParams(); +} + +RTL::~RTL() +{ +} + +void +RTL::set_home_position(const home_position_s *new_home_position) +{ + memcpy(&_home_position, new_home_position, sizeof(home_position_s)); +} + +bool +RTL::get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item) +{ + /* Open mavlink fd */ + if (_mavlink_fd < 0) { + /* try to open the mavlink log device every once in a while */ + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + } + + /* decide if we need climb */ + if (_rtl_state == RTL_STATE_NONE) { + if (global_position->alt < _home_position.alt + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + } else { + _rtl_state = RTL_STATE_RETURN; + } + } + + /* if switching directly to return state, set altitude setpoint to current altitude */ + if (_rtl_state == RTL_STATE_RETURN) { + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = global_position->alt; + } + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + + float climb_alt = _home_position.alt + _param_return_alt.get(); + + /* TODO understand and fix this */ + // if (_vstatus.condition_landed) { + // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); + // } + + new_mission_item->lat = global_position->lat; + new_mission_item->lon = global_position->lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = climb_alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_TAKEOFF; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _home_position.alt)); + break; + } + case RTL_STATE_RETURN: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + + /* TODO: add this again */ + // don't change altitude + // if (_pos_sp_triplet.previous.valid) { + // /* if previous setpoint is valid then use it to calculate heading to home */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, new_mission_item->lat, new_mission_item->lon); + + // } else { + // /* else use current position */ + // new_mission_item->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, new_mission_item->lat, new_mission_item->lon); + // } + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_DESCEND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt + _param_descend_alt.get(); + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_WAYPOINT; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = _param_land_delay.get() > -0.001f; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %d meters above home", + (int)(new_mission_item->altitude - _home_position.alt)); + break; + } + + case RTL_STATE_LAND: { + + new_mission_item->lat = _home_position.lat; + new_mission_item->lon = _home_position.lon; + new_mission_item->altitude_is_relative = false; + new_mission_item->altitude = _home_position.alt; + new_mission_item->yaw = NAN; + new_mission_item->loiter_radius = _loiter_radius; + new_mission_item->loiter_direction = 1; + new_mission_item->nav_cmd = NAV_CMD_LAND; + new_mission_item->acceptance_radius = _acceptance_radius; + new_mission_item->time_inside = 0.0f; + new_mission_item->pitch_min = 0.0f; + new_mission_item->autocontinue = true; + new_mission_item->origin = ORIGIN_ONBOARD; + + mavlink_log_info(_mavlink_fd, "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_FINISHED: { + /* nothing to do, report fail */ + return false; + } + + default: + return false; + } + + return true; +} + +bool +RTL::get_next_rtl_item(mission_item_s *new_mission_item) +{ + /* TODO implement */ + return false; +} + +void +RTL::move_to_next() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < 0) { + _rtl_state = RTL_STATE_FINISHED; + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_FINISHED; + break; + + case RTL_STATE_FINISHED: + break; + + default: + break; + } +} \ No newline at end of file diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h new file mode 100644 index 000000000..c761837fc --- /dev/null +++ b/src/modules/navigator/rtl.h @@ -0,0 +1,92 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator_mission.h + * Helper class to access missions + * @author Julian Oes + * @author Anton Babushkin + */ + +#ifndef NAVIGATOR_RTL_H +#define NAVIGATOR_RTL_H + +#include +#include + +#include +#include +#include + +class RTL : public control::SuperBlock +{ +public: + /** + * Constructor + */ + RTL(); + + /** + * Destructor + */ + ~RTL(); + + void set_home_position(const home_position_s *home_position); + + bool get_current_rtl_item(const vehicle_global_position_s *global_position, mission_item_s *new_mission_item); + bool get_next_rtl_item(mission_item_s *mission_item); + + void move_to_next(); + +private: + int _mavlink_fd; + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LAND, + RTL_STATE_FINISHED, + } _rtl_state; + + home_position_s _home_position; + float _loiter_radius; + float _acceptance_radius; + + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c new file mode 100644 index 000000000..7a8b1d745 --- /dev/null +++ b/src/modules/navigator/rtl_params.c @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes + */ + +#include + +#include + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index f2709d29f..38378651b 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 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 @@ -35,6 +35,7 @@ * @file state_table.h * * Finite-State-Machine helper class for state table + * @author: Julian Oes */ #ifndef __SYSTEMLIB_STATE_TABLE_H @@ -43,7 +44,7 @@ class StateTable { public: - typedef void (StateTable::*Action)(); + typedef bool (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; @@ -53,17 +54,23 @@ public: : myTable(table), myNsignals(nSignals), myNstates(nStates) {} #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) static_cast(_target) + #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} void dispatch(unsigned const sig) { - register Tran const *t = myTable + myState*myNsignals + sig; - (this->*(t->action))(); - - myState = t->nextState; + /* get transition using state table */ + Tran const *t = myTable + myState*myNsignals + sig; + /* first up change state, this allows to do further dispatchs in the state functions */ + + /* now execute state function, if it runs with success, accept new state */ + if ((this->*(t->action))()) { + myState = t->nextState; + } + } + bool doNothing() { + return true; } - void doNothing() {} protected: unsigned myState; private: diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index dfc461ae4..4532b329d 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -93,6 +93,8 @@ struct mission_item_s { bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ int do_jump_mission_index; /**< the mission index that we want to jump to */ + int do_jump_repeat_count; /**< how many times the jump should be repeated */ + int do_jump_current_count; /**< how many times the jump has already been repeated */ }; struct mission_s diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 34aaa30dd..85e8ef8a5 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -45,7 +45,6 @@ #include #include #include "../uORB.h" -#include /** * @addtogroup topics @@ -74,6 +73,17 @@ struct position_setpoint_s float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ }; +typedef enum { + NAV_STATE_NONE_ON_GROUND = 0, + NAV_STATE_NONE_IN_AIR, + NAV_STATE_AUTO_ON_GROUND, + NAV_STATE_LOITER, + NAV_STATE_MISSION, + NAV_STATE_RTL, + NAV_STATE_LAND, + NAV_STATE_MAX +} nav_state_t; + /** * Global position setpoint triplet in WGS84 coordinates. * diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index adcd028f0..cfab695f8 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -36,7 +36,7 @@ * Definition of the global fused WGS84 position uORB topic. * * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes * @author Lorenz Meier */ @@ -66,16 +66,16 @@ struct vehicle_global_position_s { bool global_valid; /**< true if position satisfies validity criteria of estimator */ bool baro_valid; /**< true if baro_alt is valid (vel_d is also valid in this case) */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ - float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ + float baro_alt; /**< Barometric altitude (not raw baro but fused with accelerometer) */ }; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 48af4c9e2..c1bd828d8 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -54,8 +54,6 @@ #include #include "../uORB.h" -#include - /** * @addtogroup topics @{ */ @@ -93,6 +91,14 @@ typedef enum { FAILSAFE_STATE_MAX } failsafe_state_t; +typedef enum { + NAVIGATION_STATE_NONE = 0, + NAVIGATION_STATE_MISSION, + NAVIGATION_STATE_LOITER, + NAVIGATION_STATE_RTL, + NAVIGATION_STATE_LAND +} navigation_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -152,8 +158,7 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - unsigned int set_nav_state; /**< set navigation state machine to specified value */ - uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ + navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ -- cgit v1.2.3 From db15e2811ea01dd023ae930e6e7a73c1a370cecf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:36:32 +0200 Subject: commander: Fix altitude initialisation, do not depend on global pos valid flag. --- src/modules/commander/commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ac7aff0f..077184bfb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1088,13 +1088,12 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ home.lat = gps_position.lat / (double)1e7; home.lon = gps_position.lon / (double)1e7; - home.alt = gps_position.alt; + home.alt = gps_position.alt / (float)1e3; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); -- cgit v1.2.3 From a0c922704457b540b236f1c72ed5630662da9d9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:38:15 +0200 Subject: commander: Also publish battery status in HIL, since we have a fake battery available and the system freaks out without knowing its main supply --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef..819dfbe83 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(battery_status), battery_sub, &battery); /* only consider battery voltage if system has been running 2s and battery voltage is valid */ - if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) { status.battery_voltage = battery.voltage_filtered_v; status.battery_current = battery.current_a; status.condition_battery_voltage_valid = true; -- cgit v1.2.3 From 7e621070ca0f002e2e1ccd863c31a24166ece0c2 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 18:23:27 -0700 Subject: renamed mission_switch to loiter_switch and assisted_switch to easy_switch --- src/modules/commander/commander.cpp | 10 +++++----- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 24 +++++++++++------------ src/modules/uORB/topics/manual_control_setpoint.h | 4 ++-- src/modules/uORB/topics/rc_channels.h | 4 ++-- 5 files changed, 23 insertions(+), 23 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fa..ee818d0f5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { /* stick is in MISSION position */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); @@ -1296,7 +1296,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1528,7 +1528,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->assisted_switch == SWITCH_POS_ON) { + if (sp_man->easy_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) { @@ -1545,7 +1545,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this mode } - if (sp_man->assisted_switch != SWITCH_POS_ON) { + if (sp_man->easy_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c04e176a1..a34f81923 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -620,7 +620,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); /** * Mission switch channel mapping. @@ -629,7 +629,7 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 04b74a6f5..469fc5ca0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -253,8 +253,8 @@ private: int rc_map_mode_sw; int rc_map_return_sw; - int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_easy_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,8 +296,8 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; - param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_easy_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -507,8 +507,8 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -650,11 +650,11 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -681,8 +681,8 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1415,8 +1415,8 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.easy_switch = get_rc_switch_position(EASY); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b2..b6257e22a 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,8 +78,8 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd..d99203ff6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,8 +64,8 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, - ASSISTED = 6, - MISSION = 7, + EASY = 6, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, -- cgit v1.2.3 From 831a7c4a833c68b1d418344e2f3aae2c80894b1a Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 20:53:07 -0700 Subject: Added RC_MAP_FAILSAFE parameter for customizing failsafe channel. Default to THROTTLE --- src/modules/commander/commander.cpp | 2 +- src/modules/sensors/sensors.cpp | 51 +++++++++++++++++++++++++++---------- 2 files changed, 39 insertions(+), 14 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee818d0f5..43a4ed8ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1230,7 +1230,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { - /* MISSION switch */ + /* LOITER switch */ if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 469fc5ca0..8b9aee795 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -175,7 +175,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func); /** * Gather and publish RC input data. @@ -250,6 +251,7 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; @@ -293,6 +295,7 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; @@ -499,6 +502,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); @@ -642,6 +646,10 @@ Sensors::parameters_update() warnx(paramerr); } + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx(paramerr); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx(paramerr); } @@ -1275,7 +1283,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } switch_pos_t -Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func) { if (_rc.function[func] >= 0) { float value = _rc.chan[_rc.function[func]].scaled; @@ -1294,6 +1302,23 @@ Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) } } +switch_pos_t +Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else { + return SWITCH_POS_OFF; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1318,13 +1343,13 @@ Sensors::rc_poll() /* signal looks good */ signal_lost = false; - /* check throttle failsafe */ - int8_t thr_ch = _rc.function[THROTTLE]; - if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { - /* throttle failsafe configured */ - if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { - /* throttle failsafe triggered, signal is lost by receiver */ + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; + if (_parameters.rc_fs_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fs_thr)) { + /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } } @@ -1414,10 +1439,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_switch_position(MODE); - manual.easy_switch = get_rc_switch_position(EASY); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); + manual.mode_switch = get_rc_sw3pos_position(MODE); + manual.easy_switch = get_rc_sw2pos_position(EASY); + manual.loiter_switch = get_rc_sw2pos_position(LOITER); + manual.return_switch = get_rc_sw2pos_position(RETURN); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { -- cgit v1.2.3 From 56592ec77d3863f135c10619b15a0591f957fdbf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 23 Apr 2014 19:01:05 +0200 Subject: commander: don't start RTL on failsafe if landed --- src/modules/commander/commander.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 271209412..ac433b1c7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1015,7 +1015,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - /* update subsystem */ + /* update position setpoint triplet */ orb_check(pos_sp_triplet_sub, &updated); if (updated) { @@ -1273,10 +1273,15 @@ int commander_thread_main(int argc, char *argv[]) } else { /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + transition_result_t res = TRANSITION_DENIED; + + if (!status.condition_landed) { + /* vehicle is not landed, try to perform RTL */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + } if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ + /* RTL not allowed (no global position estimate) or not wanted, try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); if (res == TRANSITION_DENIED) { -- cgit v1.2.3 From 63c50676f9757f18dfca9ec3735ce59a045cea33 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 24 Apr 2014 22:38:19 +0200 Subject: MISSION switch renamed to LOITER --- src/modules/commander/commander.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 12 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- 5 files changed, 12 insertions(+), 12 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ac433b1c7..67b9d16e8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { /* stick is in MISSION position */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a1f2a4ad5..bc49f5c85 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -594,13 +594,13 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); /** - * Mission switch channel mapping. + * Loiter switch channel mapping. * * @min 0 * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ba233dfd0..41e2148ac 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -254,7 +254,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -297,7 +297,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; param_t rc_map_assisted_sw; - param_t rc_map_mission_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -508,7 +508,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -654,7 +654,7 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -682,7 +682,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; - _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1416,7 +1416,7 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b2..a23d89cd2 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -79,7 +79,7 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd..c168b2fac 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -65,7 +65,7 @@ enum RC_CHANNELS_FUNCTION { MODE = 4, RETURN = 5, ASSISTED = 6, - MISSION = 7, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, -- cgit v1.2.3 From 2453b354faa5a6ccdcca6afb94b54967679cc9de Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 25 Apr 2014 22:26:51 +0200 Subject: Failsafe landing without position control fixed --- src/modules/commander/commander.cpp | 7 +++++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++++++++++ 2 files changed, 17 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1827f252c..0bf752279 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1747,8 +1747,11 @@ set_control_mode() control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; + + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 6b797f222..5194ef697 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1001,6 +1001,18 @@ MulticopterPositionControl::task_main() _att_sp.roll_body = euler(0); _att_sp.pitch_body = euler(1); /* yaw already used to construct rot matrix, but actual rotation matrix can have different yaw near singularity */ + + } else if (!_control_mode.flag_control_manual_enabled) { + /* autonomous altitude control without position control (failsafe landing), + * force level attitude, don't change yaw */ + R.from_euler(0.0f, 0.0f, _att_sp.yaw_body); + + /* copy rotation matrix to attitude setpoint topic */ + memcpy(&_att_sp.R_body[0][0], R.data, sizeof(_att_sp.R_body)); + _att_sp.R_valid = true; + + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; } _att_sp.thrust = thrust_abs; -- cgit v1.2.3 From 13dfe0447ccfa4f75b551d02b5c979a6ade4c81a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:07:29 +0200 Subject: Send current local position estimate as well --- src/modules/commander/commander.cpp | 24 ++++++++++++++++-------- src/modules/uORB/topics/home_position.h | 5 ++++- 2 files changed, 20 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 352711734..dd2614c1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -927,6 +927,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* update local position estimate */ + orb_check(local_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + } + /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; @@ -952,6 +960,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -968,14 +980,6 @@ int commander_thread_main(int argc, char *argv[]) tune_positive(true); } - /* update local position estimate */ - orb_check(local_position_sub, &updated); - - if (updated) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(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); @@ -1338,6 +1342,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 08d11abae..70071130d 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -59,10 +59,13 @@ struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ + + float x; /**< X coordinate in meters */ + float y; /**< Y coordinate in meters */ + float z; /**< Z coordinate in meters */ }; /** -- cgit v1.2.3 From 6612cdab856674c4cbec53863c470cb4e96a9f7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:36:09 +0200 Subject: Let commander be less pedantic about positioning data --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 352711734..148a0aafa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ 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 POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 50000 /**< consider the local or global position estimate invalid after 50ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 -- cgit v1.2.3 From 721c90291c12405b4f4a6cf5ddc9ca8cec9f0077 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 27 Apr 2014 02:26:09 +0200 Subject: commander: some HIL commands and land detector cleanup --- src/modules/commander/commander.cpp | 130 +++++++++++++++--------------------- 1 file changed, 53 insertions(+), 77 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6f86e0c2f..13336736d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. * Author: Petri Tanskanen * Lorenz Meier * Thomas Gubler @@ -383,7 +383,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe { /* result of the command */ enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - bool ret = false; /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components @@ -395,89 +394,73 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* request to set different system mode */ switch (cmd->command) { 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; + 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; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); - - if (arming_res != TRANSITION_DENIED) { - mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { + /* reset the arming mode to disarmed */ + arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + } - } else { - mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); - } - } + // Transition the arming state + transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - if (hil_ret == OK) - ret = true; + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - if (arming_res == TRANSITION_CHANGED) - ret = true; + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); - if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { - /* use autopilot-specific mode */ - if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { - /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { + } 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 (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); - } - - } 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); - } + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); } } + } - if (main_res == TRANSITION_CHANGED) - ret = true; - - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; - break; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + break; + } + case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. @@ -503,13 +486,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe status->set_nav_state = NAVIGATION_STATE_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE status->set_nav_state = NAVIGATION_STATE_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -525,7 +506,6 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; } else { /* reject parachute depoyment not armed */ @@ -981,12 +961,10 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(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); - static bool published_condition_landed_fw = false; if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; - published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -995,12 +973,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } - } else { - if (!published_condition_landed_fw) { - status.condition_landed = false; // Fixedwing does not have a landing detector currently - published_condition_landed_fw = true; - status_changed = true; - } } /* update battery status */ @@ -1265,6 +1237,8 @@ int commander_thread_main(int argc, char *argv[]) /* LAND not allowed, set TERMINATION state */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } + } else { + status.set_nav_state = NAVIGATION_STATE_MISSION; } } else { @@ -1284,7 +1258,9 @@ int commander_thread_main(int argc, char *argv[]) /* LAND not allowed, set TERMINATION state */ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } - } + } else { + status.set_nav_state = NAVIGATION_STATE_RTL; + } } } else { -- cgit v1.2.3 From e6d332aa7cd038b00643a70d99e82d60b7ffbcf0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Apr 2014 15:50:53 +0200 Subject: Make commander less pedantic about position status --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 352711734..53ed34f46 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ 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 POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 -- cgit v1.2.3 From 6a7b104c2baad7527d35736979ddd1352bf4119d Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 13:12:28 -0700 Subject: compiles --- src/modules/commander/commander.cpp | 5 +--- src/modules/sensors/sensors.cpp | 34 +---------------------- src/modules/uORB/topics/manual_control_setpoint.h | 4 --- src/modules/uORB/topics/rc_channels.h | 4 --- 4 files changed, 2 insertions(+), 45 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d5f05c991..50380107d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1228,11 +1228,8 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { -<<<<<<< HEAD + /* LOITER switch */ -======= - /* MISSION switch */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a750db12..6449c5283 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -255,11 +255,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; -<<<<<<< HEAD int rc_map_easy_sw; -======= - int rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -313,11 +309,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; -<<<<<<< HEAD param_t rc_map_easy_sw; -======= - param_t rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -534,11 +526,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ -<<<<<<< HEAD _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); -======= - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -679,7 +667,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { @@ -690,21 +678,12 @@ Sensors::parameters_update() warnx("%s", paramerr); } -<<<<<<< HEAD if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { - warnx(paramerr); - } - - if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); -======= - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx("%s", paramerr); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { @@ -745,11 +724,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; -<<<<<<< HEAD _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; -======= - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1502,17 +1477,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ -<<<<<<< HEAD manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv); manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv); manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); -======= - manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c61987df6..b6257e22a 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,11 +78,7 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ -<<<<<<< HEAD switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ -======= - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index df651e78d..d99203ff6 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,11 +64,7 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, -<<<<<<< HEAD EASY = 6, -======= - ASSISTED = 6, ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, -- cgit v1.2.3 From 269800b48c31d78fec900b4beaf3f655a8c18730 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 14:06:00 -0700 Subject: renamed EASY to POSHOLD and SEATBELT to ALTHOLD --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 15 files changed, 96 insertions(+), 96 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b75c2297f..aa9c64502 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_EASY) + if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50380107d..a99456370 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { + /* ALTHOLD */ + main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } 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); + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTHOLD"; + main_states_str[2] = "POSHOLD"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on poshold position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->easy_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_EASY); + if (sp_man->poshold_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTHOLD + print_reject_mode(status, "POSHOLD"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->easy_switch != SWITCH_POS_ON) { - print_reject_mode(status, "SEATBELT"); + if (sp_man->poshold_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTHOLD"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTHOLD (POSHOLD likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a946543d..b440e561b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - ut_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTHOLD; + ut_assert("tranisition: manual to althold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTHOLD, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; + new_main_state = MAIN_STATE_ALTHOLD; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - ut_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSHOLD; + ut_assert("transition: manual to poshold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSHOLD, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; + new_main_state = MAIN_STATE_POSHOLD; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 2144d3460..962d2804c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ 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_ALTHOLD, + PX4_CUSTOM_MAIN_MODE_POSHOLD, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f09d586c7..21e36a87d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae07275..fafab9bfe 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df785..d5a68e21f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _althold_hold_heading; /**< heading the system should hold in althold mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* poshold mode enabled */) { - /** EASY FLIGHT **/ + /** POSHOLD FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to poshold */) { + _althold_hold_heading = _att.yaw; } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* poshold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* althold mode enabled */) { - /** SEATBELT FLIGHT **/ + /** ALTHOLD FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to althold */) { + _althold_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* althold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce1645..108ef8ad4 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_STATE_POSHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 104df4e59..015d8ad16 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes during flight. + * Limits maximum tilt in AUTO and POSHOLD modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6e..2cb4fc900 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd6e21bdd..59bd99db7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting easy mode + * Threshold for selecting poshold mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 21:47:45 -0700 Subject: Replaces poshold/althold with posctrl/altctrl --- mavlink/include/mavlink/v1.0/autoquad/autoquad.h | 4 +- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 16 files changed, 97 insertions(+), 97 deletions(-) (limited to 'src/modules/commander') diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index 93e868dc3..bd3fc66e7 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -40,8 +40,8 @@ enum AUTOQUAD_NAV_STATUS AQ_NAV_STATUS_INIT=0, /* System is initializing | */ AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ - AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ - AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_ALTCTRL=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSCTRL=8, /* Position hold engaged | */ AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index aa9c64502..974e20ca2 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a99456370..be3e6d269 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { - /* ALTHOLD */ - main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { + /* ALTCTRL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTHOLD"; - main_states_str[2] = "POSHOLD"; + main_states_str[1] = "ALTCTRL"; + main_states_str[2] = "POSCTRL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on poshold position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on posctrl position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->poshold_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSHOLD); + if (sp_man->posctrl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTHOLD - print_reject_mode(status, "POSHOLD"); + // else fallback to ALTCTRL + print_reject_mode(status, "POSCTRL"); } - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->poshold_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTHOLD"); + if (sp_man->posctrl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTRL"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTHOLD (POSHOLD likely will not work too) + // else fallback to ALTCTRL (POSCTRL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b440e561b..18586053b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to ALTHOLD. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTHOLD; - ut_assert("tranisition: manual to althold", + new_main_state = MAIN_STATE_ALTCTRL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); - // MANUAL to ALTHOLD, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTHOLD; + new_main_state = MAIN_STATE_ALTCTRL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to POSHOLD. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSHOLD; - ut_assert("transition: manual to poshold", + new_main_state = MAIN_STATE_POSCTRL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); - // MANUAL to POSHOLD, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSHOLD; + new_main_state = MAIN_STATE_POSCTRL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 962d2804c..e6274fb89 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTHOLD, - PX4_CUSTOM_MAIN_MODE_POSHOLD, + PX4_CUSTOM_MAIN_MODE_ALTCTRL, + PX4_CUSTOM_MAIN_MODE_POSCTRL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21e36a87d..3b6d95135 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index fafab9bfe..dc82ee475 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d5a68e21f..024dd98ec 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _althold_hold_heading; /**< heading the system should hold in althold mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* poshold mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** POSHOLD FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to poshold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* poshold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* althold mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** ALTHOLD FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to althold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* althold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 108ef8ad4..38a5433ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTHOLD) { + } else if (status->main_state == MAIN_STATE_ALTCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; - } else if (status->main_state == MAIN_STATE_POSHOLD) { + } else if (status->main_state == MAIN_STATE_POSCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 015d8ad16..dacdd46f0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and POSHOLD modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 2cb4fc900..6d46db9bd 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD) { + _status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 59bd99db7..02890b452 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting poshold mode + * Threshold for selecting posctrl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Tue, 29 Apr 2014 20:51:04 -0700 Subject: shortened rc_assisted_th to rc_assist_th in case we add a dedicated switch mapping later --- src/modules/commander/commander.cpp | 6 +++--- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 16 ++++++++-------- 3 files changed, 13 insertions(+), 13 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index be3e6d269..ad15750c0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1145,7 +1145,7 @@ 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_READY mode or (ASSISTED mode and landed) -> disarm + /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST 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) && @@ -1299,7 +1299,7 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on posctrl position */ + /* flight termination in manual mode if assist switch is on posctrl position */ if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); @@ -1556,7 +1556,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin // TRANSITION_DENIED is not possible here break; - case SWITCH_POS_MIDDLE: // ASSISTED + case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctrl_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_POSCTRL); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 02890b452..873fff872 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -671,7 +671,7 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); PARAM_DEFINE_INT32(RC_FAILS_THR, 0); /** - * Threshold for selecting assisted mode + * Threshold for selecting assist mode * * min:-1 * max:+1 @@ -684,7 +684,7 @@ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); * negative : true when channel Date: Wed, 30 Apr 2014 15:33:47 +0200 Subject: commander: Stop mixing board support and high level code - just accept that non-mandatory leds may or may not be there --- src/modules/commander/commander_helper.cpp | 16 +++------------- 1 file changed, 3 insertions(+), 13 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index fe6c9bfaa..0fd3c9e9e 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -199,15 +199,9 @@ int led_init() } /* 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 + (void)ioctl(leds, LED_ON, LED_BLUE); + /* we consider the amber led mandatory */ if (ioctl(leds, LED_ON, LED_AMBER)) { warnx("Amber LED: ioctl fail\n"); return ERROR; @@ -217,11 +211,7 @@ int led_init() 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 + warnx("No RGB LED found at " RGBLED_DEVICE_PATH); } return 0; -- cgit v1.2.3 From 1d6b9fae037422f4c61bdd7ee1a5ea0803a59726 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 May 2014 14:57:06 +0200 Subject: Fix in-air restarts, protect against an external MAVLink sender exploiting the restart mechanism --- src/drivers/px4io/px4io.cpp | 24 ++++++++++++++++++++---- src/modules/commander/commander.cpp | 5 +++++ src/modules/mavlink/mavlink_receiver.cpp | 6 ++++++ 3 files changed, 31 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8458c2fdb..aec6dd3b7 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -683,6 +683,25 @@ PX4IO::init() /* send command to arm system via command API */ vehicle_command_s cmd; + /* send this to itself */ + param_t sys_id_param = param_find("MAV_SYS_ID"); + param_t comp_id_param = param_find("MAV_COMP_ID"); + + int32_t sys_id; + int32_t comp_id; + + if (param_get(sys_id_param, &sys_id)) { + errx(1, "PRM SYSID"); + } + + if (param_get(comp_id_param, &comp_id)) { + errx(1, "PRM CMPID"); + } + + cmd.target_system = sys_id; + cmd.target_component = comp_id; + cmd.source_system = sys_id; + cmd.source_component = comp_id; /* request arming */ cmd.param1 = 1.0f; cmd.param2 = 0; @@ -692,10 +711,7 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - // cmd.target_system = status.system_id; - // cmd.target_component = status.component_id; - // cmd.source_system = status.system_id; - // cmd.source_component = status.component_id; + /* ask to confirm command */ cmd.confirmation = 1; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 53ed34f46..141b371b3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -484,6 +484,11 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); } else { + + // Flick to inair restore first if this comes from an onboard system + if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { + status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + } transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7c93c1c00..64fc41838 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -217,6 +217,12 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) _mavlink->_task_should_exit = true; } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP ID"); + return; + } + struct vehicle_command_s vcmd; memset(&vcmd, 0, sizeof(vcmd)); -- cgit v1.2.3 From 32b84b9652b5636ba4a2a30caae6fffacb47f1d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 6 May 2014 21:46:59 +0200 Subject: Much better failsafe reporting on failsafe state changes --- src/modules/commander/commander.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 141b371b3..99a644c9d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1271,6 +1271,13 @@ int commander_thread_main(int argc, char *argv[]) if (res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); + } + + } else if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); } } @@ -1281,15 +1288,25 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_landed) { /* vehicle is not landed, try to perform RTL */ res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND"); + } } if (res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate) or not wanted, try LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); + } + if (res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else if (res == TRANSITION_CHANGED) { + mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); } } } -- cgit v1.2.3 From 0c318f9ad7ab9471786a1f0fa29a599f65b5d549 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 7 May 2014 08:42:57 +0200 Subject: commander: Fix the position failsafe to a) use proper logic to determine if eph / epv are good or not (the previous logic was in some states not initialized) and b) add a hysteresis time - because the check as it was before had zero hysteresis time for a bad eph / epv value --- src/modules/commander/commander.cpp | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 141b371b3..aa4e643dc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,9 +117,10 @@ 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 POSITION_TIMEOUT 100000 /**< consider the local or global position estimate invalid after 100ms */ -#define RC_TIMEOUT 500000 -#define DIFFPRESS_TIMEOUT 2000000 +#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ +#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ +#define RC_TIMEOUT 500000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -746,8 +747,9 @@ int commander_thread_main(int argc, char *argv[]) bool low_battery_voltage_actions_done = false; bool critical_battery_voltage_actions_done = false; - uint64_t last_idle_time = 0; - uint64_t start_time = 0; + hrt_abstime last_idle_time = 0; + hrt_abstime start_time = 0; + hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -776,6 +778,9 @@ 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)); + /* Init EPH and EPV */ + global_position.eph = 1000.0f; + global_position.epv = 1000.0f; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -938,11 +943,15 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_global_position_valid) { if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { eph_epv_good = false; + } else { + eph_epv_good = true; } } else { if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { eph_epv_good = true; + } else { + eph_epv_good = false; } } check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); @@ -1264,7 +1273,12 @@ int commander_thread_main(int argc, char *argv[]) /* check if AUTO mode still allowed */ transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_DENIED) { + if (res == TRANSITION_NOT_CHANGED) { + last_auto_state_valid = hrt_absolute_time(); + } + + /* still invalid state after the timeout interval, execute failsafe */ + if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (res == TRANSITION_DENIED)) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); -- cgit v1.2.3 From 88b18bbad1be31cf31ff964c6cf6f3123948488d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 11 May 2014 13:35:05 +0200 Subject: ALTCTRL/POSCTRL renamed to ALTCTL/POSCTL --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 12 +++--- src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 47 insertions(+), 47 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 974e20ca2..c41d8f242 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5124a31f..8c7f6270d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -435,13 +435,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { - /* ALTCTRL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { + /* ALTCTL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -456,8 +456,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSCTRL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTRL); + /* POSCTL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -634,8 +634,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTRL"; - main_states_str[2] = "POSCTRL"; + main_states_str[1] = "ALTCTL"; + main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1335,8 +1335,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assist switch is on posctrl position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assist switch is on POSCTL position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1593,25 +1593,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSIST - if (sp_man->posctrl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTRL); + if (sp_man->posctl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTCTRL - print_reject_mode(status, "POSCTRL"); + // else fallback to ALTCTL + print_reject_mode(status, "POSCTL"); } - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->posctrl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTRL"); + if (sp_man->posctl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTL"); } // else fallback to MANUAL @@ -1626,9 +1626,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTRL (POSCTRL likely will not work too) + // else fallback to ALTCTL (POSCTL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTCTRL); + res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1674,7 +1674,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1685,7 +1685,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 18586053b..ee0d08391 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -320,15 +320,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTRL; + new_main_state = MAIN_STATE_ALTCTL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); @@ -336,15 +336,15 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTRL; + new_main_state = MAIN_STATE_POSCTL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e6274fb89..a83c81850 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTCTRL, - PX4_CUSTOM_MAIN_MODE_POSCTRL, + PX4_CUSTOM_MAIN_MODE_ALTCTL, + PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3b6d95135..97a214c33 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTCTRL: + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSCTRL: + case MAIN_STATE_POSCTL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index 596e286a4..bbb39f20f 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement pos control */) { + } else if (_status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 9a7e636c3..b8879157e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTCTRL) { + } else if (status->main_state == MAIN_STATE_ALTCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; - } else if (status->main_state == MAIN_STATE_POSCTRL) { + } else if (status->main_state == MAIN_STATE_POSCTL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 6d46db9bd..91230a37c 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTCTRL || - _status.main_state == MAIN_STATE_POSCTRL) { + _status.main_state == MAIN_STATE_ALTCTL || + _status.main_state == MAIN_STATE_POSCTL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 727be9492..ac394786d 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -77,8 +77,8 @@ struct manual_control_setpoint_s { float aux5; /**< default function: payload drop */ switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ - switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): no effect, return */ - switch_pos_t posctrl_switch; /**< posctrl 2 position switch (optional): altctrl, posctrl */ + switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): normal, return */ + switch_pos_t posctl_switch; /**< posctrl 2 position switch (optional): altctl, posctl */ switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index f401140ae..f56355246 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,8 +63,8 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, - MAIN_STATE_ALTCTRL, - MAIN_STATE_POSCTRL, + MAIN_STATE_ALTCTL, + MAIN_STATE_POSCTL, MAIN_STATE_AUTO, MAIN_STATE_MAX } main_state_t; -- cgit v1.2.3 From 349809f5353d70eb9d569a267165e0f1b2054e02 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 11 May 2014 13:36:51 +0200 Subject: sensors, commander: code style fixed --- .../commander/accelerometer_calibration.cpp | 66 +++++---- src/modules/commander/airspeed_calibration.cpp | 6 +- src/modules/commander/commander.cpp | 157 +++++++++++++-------- src/modules/commander/commander_helper.cpp | 20 ++- src/modules/commander/gyro_calibration.cpp | 18 +-- src/modules/commander/mag_calibration.cpp | 31 ++-- src/modules/commander/state_machine_helper.cpp | 156 ++++++++++---------- src/modules/sensors/sensors.cpp | 102 ++++++++----- 8 files changed, 345 insertions(+), 211 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 1cbdf9bf8..7180048ff 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd) 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<3,3> board_rotation; + math::Matrix<3, 3> board_rotation; get_rot_matrix(board_rotation_id, &board_rotation); - math::Matrix<3,3> board_rotation_t = board_rotation.transposed(); + math::Matrix<3, 3> board_rotation_t = board_rotation.transposed(); math::Vector<3> accel_offs_vec(&accel_offs[0]); - math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec; - math::Matrix<3,3> accel_T_mat(&accel_T[0][0]); - math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation; + math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec; + math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]); + math::Matrix<3, 3> 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); @@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float } } - if (old_done_count != done_count) + if (old_done_count != done_count) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count); + } - if (done) + if (done) { break; + } mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", (!data_collected[0]) ? "x+ " : "", @@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); - if (d > still_thr2 * 8.0f) + if (d > still_thr2 * 8.0f) { d = still_thr2 * 8.0f; + } - if (d > accel_disp[i]) + if (d > accel_disp[i]) { accel_disp[i] = d; + } } /* still detector with hysteresis */ @@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 0; // [ g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 0; // [ g, 0, 0 ] + } if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 1; // [ -g, 0, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 1; // [ -g, 0, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 2; // [ 0, g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 2; // [ 0, g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabsf(accel_ema[2]) < accel_err_thr) - return 3; // [ 0, -g, 0 ] + fabsf(accel_ema[2]) < accel_err_thr) { + return 3; // [ 0, -g, 0 ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) - return 4; // [ 0, 0, g ] + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) { + return 4; // [ 0, 0, g ] + } if (fabsf(accel_ema[0]) < accel_err_thr && fabsf(accel_ema[1]) < accel_err_thr && - fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) - return 5; // [ 0, 0, -g ] + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) { + return 5; // [ 0, 0, -g ] + } mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation"); @@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { accel_sum[i] += sensor.accelerometer_m_s2[i]; + } count++; @@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp continue; } - if (errcount > samples_num / 10) + if (errcount > samples_num / 10) { return ERROR; + } } for (int i = 0; i < 3; i++) { @@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) - return ERROR; // Singular matrix + if (det == 0.0f) { + return ERROR; // Singular matrix + } dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; @@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo /* calculate inverse matrix for A */ float mat_A_inv[3][3]; - if (mat_invert3(mat_A, mat_A_inv) != OK) + if (mat_invert3(mat_A, mat_A_inv) != OK) { return ERROR; + } /* copy results to accel_T */ for (int i = 0; i < 3; i++) { diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index c8c7a42e7..5d21d89d0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd) bool paramreset_successful = false; int fd = open(AIRSPEED_DEVICE_PATH, 0); + if (fd > 0) { if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { paramreset_successful = true; + } else { mavlink_log_critical(mavlink_fd, "airspeed offset zero failed"); } + close(fd); } @@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8c7f6270d..612393bc1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -221,7 +221,7 @@ void print_status(); 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); -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy); +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -233,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul int commander_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { usage("missing command"); + } if (!strcmp(argv[1], "start")) { @@ -261,8 +262,9 @@ int commander_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { - if (!thread_running) + if (!thread_running) { errx(0, "commander already stopped"); + } thread_should_exit = true; @@ -304,8 +306,9 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason) + if (reason) { fprintf(stderr, "%s\n", reason); + } fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); exit(1); @@ -364,20 +367,22 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) { - transition_result_t arming_res = TRANSITION_NOT_CHANGED; - - // Transition the armed state. By passing mavlink_fd to arming_state_transition it will - // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); - if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); - } else if (arming_res == TRANSITION_DENIED) { - tune_negative(true); - } - - return arming_res; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will + // output appropriate error messages if the state cannot transition. + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + + if (arming_res == TRANSITION_CHANGED && mavlink_fd) { + mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + + } else if (arming_res == TRANSITION_DENIED) { + tune_negative(true); + } + + return arming_res; } bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) @@ -417,14 +422,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (hil_ret == OK) + if (hil_ret == OK) { ret = true; + } - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + // Transition the arming state + arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - if (arming_res == TRANSITION_CHANGED) + if (arming_res == TRANSITION_CHANGED) { ret = true; + } /* set main state */ transition_result_t main_res = TRANSITION_DENIED; @@ -466,8 +473,9 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } - if (main_res == TRANSITION_CHANGED) + if (main_res == TRANSITION_CHANGED) { ret = true; + } if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -480,24 +488,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { + // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. + // We use an float epsilon delta to test float equality. + if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); - } else { - // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; - } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); - if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } - } + } else { + + // Flick to inair restore first if this comes from an onboard system + if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { + status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + } + + transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + + if (arming_res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + + } else { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } + } } break; @@ -525,7 +537,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; - /* Flight termination */ + /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command //XXX: to enable the parachute, a param needs to be set @@ -545,6 +557,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; + if (use_current) { /* use current position */ if (status->condition_global_position_valid) { @@ -588,6 +601,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } } break; + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: @@ -601,6 +615,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED); break; } + if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ answer_command(*cmd, result); @@ -883,6 +898,7 @@ int commander_thread_main(int argc, char *argv[]) /* re-check RC calibration */ rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); } + /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); @@ -923,6 +939,7 @@ int commander_thread_main(int argc, char *argv[]) /* disarm if safety is now on and still armed */ if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); } @@ -940,9 +957,11 @@ int commander_thread_main(int argc, char *argv[]) /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; + if (status.condition_global_position_valid) { if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) { eph_epv_good = false; + } else { eph_epv_good = true; } @@ -950,17 +969,19 @@ int commander_thread_main(int argc, char *argv[]) } else { if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) { eph_epv_good = true; + } else { eph_epv_good = false; } } + check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { home.lat = global_position.lat; home.lon = global_position.lon; @@ -995,6 +1016,7 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); static bool published_condition_landed_fw = false; + if (status.is_rotary_wing && status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; @@ -1008,6 +1030,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } + } else { if (!published_condition_landed_fw) { status.condition_landed = false; // Fixedwing does not have a landing detector currently @@ -1077,8 +1100,9 @@ int commander_thread_main(int argc, char *argv[]) /* compute system load */ uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (last_idle_time > 0) - status.load = 1.0f - ((float)interval_runtime / 1e6f); //system load is time spent in non-idle + if (last_idle_time > 0) { + 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; @@ -1255,7 +1279,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { + pos_sp_triplet.nav_state == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); @@ -1320,6 +1344,7 @@ int commander_thread_main(int argc, char *argv[]) if (res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } else if (res == TRANSITION_CHANGED) { mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); } @@ -1350,8 +1375,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) + if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) { status_changed = true; + } } /* check which state machines for changes, clear "changed" flag */ @@ -1368,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[]) /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid && - (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { + (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) { // TODO remove code duplication home.lat = global_position.lat; @@ -1390,6 +1416,7 @@ int commander_thread_main(int argc, char *argv[]) status.condition_home_position_valid = true; } } + was_armed = armed.armed; if (main_state_changed) { @@ -1553,21 +1580,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a } else if (actuator_armed->ready_to_arm) { /* ready to arm, blink at 1Hz */ - if (leds_counter % 20 == 0) + if (leds_counter % 20 == 0) { led_toggle(LED_BLUE); + } } else { /* not ready to arm, blink at 10Hz */ - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_BLUE); + } } #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { - if (leds_counter % 2 == 0) + if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); + } } else { led_off(LED_AMBER); @@ -1781,7 +1811,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul { switch (result) { case VEHICLE_CMD_RESULT_ACCEPTED: - tune_positive(true); + tune_positive(true); break; case VEHICLE_CMD_RESULT_DENIED: @@ -1831,8 +1861,9 @@ void *commander_low_prio_loop(void *arg) int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200); /* timed out - periodic check for thread_should_exit, etc. */ - if (pret == 0) + if (pret == 0) { continue; + } /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { @@ -1847,8 +1878,9 @@ void *commander_low_prio_loop(void *arg) if (cmd.command == VEHICLE_CMD_DO_SET_MODE || cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM || cmd.command == VEHICLE_CMD_NAV_TAKEOFF || - cmd.command == VEHICLE_CMD_DO_SET_SERVO) + cmd.command == VEHICLE_CMD_DO_SET_SERVO) { continue; + } /* only handle low-priority commands here */ switch (cmd.command) { @@ -1926,6 +1958,7 @@ void *commander_low_prio_loop(void *arg) /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); + } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { @@ -1940,10 +1973,12 @@ void *commander_low_prio_loop(void *arg) } - if (calib_ret == OK) + if (calib_ret == OK) { tune_positive(true); - else + + } else { tune_negative(true); + } arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); @@ -1963,11 +1998,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1983,11 +2020,13 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "#audio: parameters save error"); /* convenience as many parts of NuttX use negative errno */ - if (ret < 0) + if (ret < 0) { ret = -ret; + } - if (ret < 1000) + if (ret < 1000) { mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret)); + } answer_command(cmd, VEHICLE_CMD_RESULT_FAILED); } @@ -1997,8 +2036,8 @@ void *commander_low_prio_loop(void *arg) } case VEHICLE_CMD_START_RX_PAIR: - /* handled in the IO driver */ - break; + /* handled in the IO driver */ + break; default: /* don't answer on unsupported commands, it will be done in main loop */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 0fd3c9e9e..86f77cdb8 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -113,17 +113,22 @@ void buzzer_deinit() close(buzzer); } -void set_tune(int tune) { +void set_tune(int tune) +{ unsigned int new_tune_duration = tune_durations[tune]; + /* don't interrupt currently playing non-repeating tune by repeating */ if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) { /* allow interrupting current non-repeating tune by the same tune */ if (tune != tune_current || new_tune_duration != 0) { ioctl(buzzer, TONE_SET_ALARM, tune); } + tune_current = tune; + if (new_tune_duration != 0) { tune_end = hrt_absolute_time() + new_tune_duration; + } else { tune_end = 0; } @@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_GREEN); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_POSITIVE_TUNE); } @@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_WHITE); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEUTRAL_TUNE); } @@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer) blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; rgbled_set_color(RGBLED_COLOR_RED); rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + if (use_buzzer) { set_tune(TONE_NOTIFY_NEGATIVE_TUNE); } @@ -244,22 +252,25 @@ int led_off(int led) void rgbled_set_color(rgbled_color_t color) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color); + } } void rgbled_set_mode(rgbled_mode_t mode) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode); + } } void rgbled_set_pattern(rgbled_pattern_t *pattern) { - if (rgbleds != -1) + if (rgbleds != -1) { ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern); + } } float battery_remaining_estimate_voltage(float voltage, float discharged) @@ -299,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged) if (bat_capacity > 0.0f) { /* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */ ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity); + } else { /* else use voltage */ ret = remaining_voltage; diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 30cd0d48d..cbc2844c1 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd) gyro_scale.z_offset += gyro_report.z; calibration_counter++; - if (calibration_counter % (calibration_count / 20) == 0) + if (calibration_counter % (calibration_count / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); + } } else { poll_errcount++; @@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd) /* apply new offsets */ fd = open(GYRO_DEVICE_PATH, 0); - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) { warn("WARNING: failed to apply new offsets for gyro"); + } close(fd); @@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd) 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; } - 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(); @@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd) //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; } - 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; diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 4ebf266f4..9296db6ed 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -124,6 +124,7 @@ int do_mag_calibration(int mavlink_fd) res = ERROR; return res; } + } else { /* exit */ return ERROR; @@ -163,8 +164,9 @@ int do_mag_calibration(int mavlink_fd) calibration_counter++; - if (calibration_counter % (calibration_maxcount / 20) == 0) + if (calibration_counter % (calibration_maxcount / 20) == 0) { mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); + } } else { poll_errcount++; @@ -198,14 +200,17 @@ int do_mag_calibration(int mavlink_fd) } } - if (x != NULL) + if (x != NULL) { free(x); + } - if (y != NULL) + if (y != NULL) { free(y); + } - if (z != NULL) + if (z != NULL) { free(z); + } if (res == OK) { /* apply calibration and set parameters */ @@ -234,23 +239,29 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { res = ERROR; + } - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { res = ERROR; + } if (res != OK) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 97a214c33..abcf72717 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -75,38 +75,38 @@ static bool failsafe_state_changed = true; // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char* state_names[ARMING_STATE_MAX] = { - "ARMING_STATE_INIT", - "ARMING_STATE_STANDBY", - "ARMING_STATE_ARMED", - "ARMING_STATE_ARMED_ERROR", - "ARMING_STATE_STANDBY_ERROR", - "ARMING_STATE_REBOOT", - "ARMING_STATE_IN_AIR_RESTORE", +static const char *state_names[ARMING_STATE_MAX] = { + "ARMING_STATE_INIT", + "ARMING_STATE_STANDBY", + "ARMING_STATE_ARMED", + "ARMING_STATE_ARMED_ERROR", + "ARMING_STATE_STANDBY_ERROR", + "ARMING_STATE_REBOOT", + "ARMING_STATE_IN_AIR_RESTORE", }; transition_result_t arming_state_transition(struct vehicle_status_s *status, /// current vehicle status - const struct safety_s *safety, /// current safety settings - arming_state_t new_arming_state, /// arming state requested - struct actuator_armed_s *armed, /// current armed status - const int mavlink_fd) /// mavlink fd for error reporting, 0 for none + const struct safety_s *safety, /// current safety settings + arming_state_t new_arming_state, /// arming state requested + struct actuator_armed_s *armed, /// current armed status + const int mavlink_fd) /// mavlink fd for error reporting, 0 for none { - // Double check that our static arrays are still valid - ASSERT(ARMING_STATE_INIT == 0); - ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); - + // Double check that our static arrays are still valid + ASSERT(ARMING_STATE_INIT == 0); + ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); + /* * Perform an atomic state update */ @@ -117,63 +117,70 @@ arming_state_transition(struct vehicle_status_s *status, /// current /* only check transition if the new state is actually different from the current one */ if (new_arming_state == status->arming_state) { ret = TRANSITION_NOT_CHANGED; + } else { /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; + } else { armed->lockdown = false; } - - // Check that we have a valid state transition - bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; - if (valid_transition) { - // We have a good transition. Now perform any secondary validation. - if (new_arming_state == ARMING_STATE_ARMED) { - // Fail transition if we need safety switch press - // Allow if coming from in air restore - // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); - } - valid_transition = false; - } - } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { - new_arming_state = ARMING_STATE_STANDBY_ERROR; - } - } - - // HIL can always go to standby - if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { - valid_transition = true; - } - - /* Sensors need to be initialized for STANDBY state */ - if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { - valid_transition = false; - } - - // Finish up the state transition - if (valid_transition) { - armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; - armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; - ret = TRANSITION_CHANGED; - status->arming_state = new_arming_state; - arming_state_changed = true; - } - } - + + // Check that we have a valid state transition + bool valid_transition = arming_transitions[new_arming_state][status->arming_state]; + + if (valid_transition) { + // We have a good transition. Now perform any secondary validation. + if (new_arming_state == ARMING_STATE_ARMED) { + // Fail transition if we need safety switch press + // Allow if coming from in air restore + // Allow if HIL_STATE_ON + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + } + + valid_transition = false; + } + + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { + new_arming_state = ARMING_STATE_STANDBY_ERROR; + } + } + + // HIL can always go to standby + if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) { + valid_transition = true; + } + + /* Sensors need to be initialized for STANDBY state */ + if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) { + valid_transition = false; + } + + // Finish up the state transition + if (valid_transition) { + armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR; + armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; + ret = TRANSITION_CHANGED; + status->arming_state = new_arming_state; + arming_state_changed = true; + } + } + /* end of atomic state update */ irqrestore(flags); - if (ret == TRANSITION_DENIED) { - static const char* errMsg = "Invalid arming transition from %s to %s"; - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } - warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } + if (ret == TRANSITION_DENIED) { + static const char *errMsg = "Invalid arming transition from %s to %s"; + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } + + warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); + } return ret; } @@ -320,6 +327,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; d = opendir("/dev"); + if (d) { struct dirent *direntry; @@ -331,26 +339,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (!strncmp("tty", direntry->d_name, 3)) { continue; } + /* skip mtd devices */ if (!strncmp("mtd", direntry->d_name, 3)) { continue; } + /* skip ram devices */ if (!strncmp("ram", direntry->d_name, 3)) { continue; } + /* skip MMC devices */ if (!strncmp("mmc", direntry->d_name, 3)) { continue; } + /* skip mavlink */ if (!strcmp("mavlink", direntry->d_name)) { continue; } + /* skip console */ if (!strcmp("console", direntry->d_name)) { continue; } + /* skip null */ if (!strcmp("null", direntry->d_name)) { continue; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a02e8187d..c561d1c0a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -220,8 +220,8 @@ private: struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; - math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ @@ -644,8 +644,9 @@ Sensors::parameters_update() } /* handle wrong values */ - if (!rc_valid) + if (!rc_valid) { warnx("WARNING WARNING WARNING\n\nRC CALIBRATION NOT SANE!\n\n"); + } const char *paramerr = "FAIL PARM LOAD"; @@ -701,19 +702,19 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); - _parameters.rc_assist_inv = (_parameters.rc_assist_th<0); + _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); _parameters.rc_assist_th = fabs(_parameters.rc_assist_th); param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th)); - _parameters.rc_auto_inv = (_parameters.rc_auto_th<0); + _parameters.rc_auto_inv = (_parameters.rc_auto_th < 0); _parameters.rc_auto_th = fabs(_parameters.rc_auto_th); param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th)); - _parameters.rc_posctl_inv = (_parameters.rc_posctl_th<0); + _parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0); _parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th); param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th)); - _parameters.rc_return_inv = (_parameters.rc_return_th<0); + _parameters.rc_return_inv = (_parameters.rc_return_th < 0); _parameters.rc_return_th = fabs(_parameters.rc_return_th); param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th)); - _parameters.rc_loiter_inv = (_parameters.rc_loiter_th<0); + _parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0); _parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th); /* update RC function mappings */ @@ -843,12 +844,14 @@ Sensors::gyro_init() #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /* set the gyro internal sampling rate up to at least 1000Hz */ - if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) + if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) { ioctl(fd, GYROIOCSSAMPLERATE, 800); + } /* set the driver to poll at 1000Hz */ - if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) + if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) { ioctl(fd, SENSORIOCSPOLLRATE, 800); + } #else @@ -903,12 +906,15 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCGEXTERNAL, 0); - if (ret < 0) + if (ret < 0) { errx(1, "FATAL: unknown if magnetometer is external or onboard"); - else if (ret == 1) + + } else if (ret == 1) { _mag_is_external = true; - else + + } else { _mag_is_external = false; + } close(fd); } @@ -1008,10 +1014,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); - if (_mag_is_external) + if (_mag_is_external) { vect = _external_mag_rotation * vect; - else + + } else { vect = _board_rotation * vect; + } raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); @@ -1129,8 +1137,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.gyro_scale[2], }; - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) { warn("WARNING: failed to set scale / offsets for gyro"); + } close(fd); @@ -1144,8 +1153,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.accel_scale[2], }; - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) { warn("WARNING: failed to set scale / offsets for accel"); + } close(fd); @@ -1159,8 +1169,9 @@ Sensors::parameter_update_poll(bool forced) _parameters.mag_scale[2], }; - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) { warn("WARNING: failed to set scale / offsets for mag"); + } close(fd); @@ -1174,8 +1185,10 @@ Sensors::parameter_update_poll(bool forced) 1.0f, }; - if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) + if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { warn("WARNING: failed to set scale / offsets for airspeed sensor"); + } + close(fd); } @@ -1193,10 +1206,12 @@ void Sensors::adc_poll(struct sensor_combined_s &raw) { /* only read if publishing */ - if (!_publishing) + if (!_publishing) { return; + } hrt_abstime t = hrt_absolute_time(); + /* rate limit to 100 Hz */ if (t - _last_adc >= 10000) { /* make space for a maximum of twelve channels (to ensure reading all channels at once) */ @@ -1221,6 +1236,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_v = voltage; + /* one-time initialization of low-pass value to avoid long init delays */ if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) { _battery_status.voltage_filtered_v = voltage; @@ -1239,19 +1255,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* handle current only if voltage is valid */ if (_battery_status.voltage_v > 0.0f) { float current = (buf_adc[i].am_data * _parameters.battery_current_scaling); + /* check measured current value */ if (current >= 0.0f) { _battery_status.timestamp = t; _battery_status.current_a = current; + if (_battery_current_timestamp != 0) { /* initialize discharged value */ - if (_battery_status.discharged_mah < 0.0f) + if (_battery_status.discharged_mah < 0.0f) { _battery_status.discharged_mah = 0.0f; + } + _battery_discharged += current * (t - _battery_current_timestamp); _battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f; } } } + _battery_current_timestamp = t; } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { @@ -1284,7 +1305,9 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } } } + _last_adc = t; + if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) { /* announce the battery status if needed, just publish else */ if (_battery_pub > 0) { @@ -1303,6 +1326,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max { if (_rc.function[func] >= 0) { float value = _rc.chan[_rc.function[func]].scaled; + if (value < min_value) { return min_value; @@ -1312,6 +1336,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } else { return value; } + } else { return 0.0f; } @@ -1322,6 +1347,7 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1342,6 +1368,7 @@ Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo { if (_rc.function[func] >= 0) { float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; @@ -1380,13 +1407,15 @@ Sensors::rc_poll() /* check failsafe */ int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle - if (_parameters.rc_map_failsafe>0){ // if not 0, use channel number instead of rc.function mapping + + if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping fs_ch = _parameters.rc_map_failsafe - 1; } + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { /* failsafe configured */ if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || - (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } @@ -1395,8 +1424,9 @@ Sensors::rc_poll() unsigned channel_limit = rc_input.channel_count; - if (channel_limit > _rc_max_chan_count) + if (channel_limit > _rc_max_chan_count) { channel_limit = _rc_max_chan_count; + } /* read out and scale values from raw message even if signal is invalid */ for (unsigned int i = 0; i < channel_limit; i++) { @@ -1404,11 +1434,13 @@ Sensors::rc_poll() /* * 1) Constrain to min/max values, as later processing depends on bounds. */ - if (rc_input.values[i] < _parameters.min[i]) + if (rc_input.values[i] < _parameters.min[i]) { rc_input.values[i] = _parameters.min[i]; + } - if (rc_input.values[i] > _parameters.max[i]) + if (rc_input.values[i] > _parameters.max[i]) { rc_input.values[i] = _parameters.max[i]; + } /* * 2) Scale around the mid point differently for lower and upper range. @@ -1440,8 +1472,9 @@ Sensors::rc_poll() _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (!isfinite(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) { _rc.chan[i].scaled = 0.0f; + } } _rc.chan_count = rc_input.channel_count; @@ -1623,8 +1656,9 @@ Sensors::task_main() diff_pres_poll(raw); /* Inform other processes that new data is available to copy */ - if (_publishing) + if (_publishing) { orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw); + } /* Look for new r/c input data */ rc_poll(); @@ -1661,18 +1695,21 @@ Sensors::start() int sensors_main(int argc, char *argv[]) { - if (argc < 1) + if (argc < 1) { errx(1, "usage: sensors {start|stop|status}"); + } if (!strcmp(argv[1], "start")) { - if (sensors::g_sensors != nullptr) + if (sensors::g_sensors != nullptr) { errx(0, "already running"); + } sensors::g_sensors = new Sensors; - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "alloc failed"); + } if (OK != sensors::g_sensors->start()) { delete sensors::g_sensors; @@ -1684,8 +1721,9 @@ int sensors_main(int argc, char *argv[]) } if (!strcmp(argv[1], "stop")) { - if (sensors::g_sensors == nullptr) + if (sensors::g_sensors == nullptr) { errx(1, "not running"); + } delete sensors::g_sensors; sensors::g_sensors = nullptr; -- cgit v1.2.3 From dd04a70afa94980dc38a82c592dc61e062c3f853 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 11 May 2014 18:10:02 +0200 Subject: Reporting cleanup, use different variables for different state switching results to avoid being tripped on local / global name scope --- src/modules/commander/commander.cpp | 64 +++++++++++++------------------------ 1 file changed, 23 insertions(+), 41 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 612393bc1..22504642f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1178,10 +1178,10 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t res; // store all transitions results here + transition_result_t arming_res; // store all transitions results here /* arm/disarm by RC */ - res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ @@ -1193,7 +1193,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); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1215,7 +1215,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); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1228,7 +1228,7 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (res == TRANSITION_CHANGED) { + if (arming_res == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); @@ -1236,24 +1236,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - } else if (res == TRANSITION_DENIED) { + } else if (arming_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* recover from failsafe */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } /* evaluate the main state machine according to mode switches */ - res = set_main_state_rc(&status, &sp_man); + transition_result_t main_res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ - if (res == TRANSITION_CHANGED) { + if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); - } else if (res == TRANSITION_DENIED) { + } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } @@ -1296,57 +1296,39 @@ int commander_thread_main(int argc, char *argv[]) if (armed.armed) { if (status.main_state == MAIN_STATE_AUTO) { /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - if (res == TRANSITION_NOT_CHANGED) { + if (auto_res == TRANSITION_NOT_CHANGED) { last_auto_state_valid = hrt_absolute_time(); } /* still invalid state after the timeout interval, execute failsafe */ - if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (res == TRANSITION_DENIED)) { + if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { + if (auto_res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); - } - - } else if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } else { /* failsafe for manual modes */ - transition_result_t res = TRANSITION_DENIED; + transition_result_t manual_res = TRANSITION_DENIED; if (!status.condition_landed) { /* vehicle is not landed, try to perform RTL */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND"); - } + manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); } - if (res == TRANSITION_DENIED) { + if (manual_res == TRANSITION_DENIED) { /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING"); - } + manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - if (res == TRANSITION_DENIED) { + if (manual_res == TRANSITION_DENIED) { /* LAND not allowed, set TERMINATION state */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - - } else if (res == TRANSITION_CHANGED) { - mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION"); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); } } } @@ -1354,7 +1336,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { /* reset failsafe when disarmed */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); } } } -- cgit v1.2.3 From cde4c9addbe2e8ccd782c53daf519fcf9669626a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 12 May 2014 09:21:27 +0200 Subject: commander: use new manual control setpoint variable names --- src/modules/commander/commander.cpp | 8 ++++---- src/modules/commander/rc_calibration.cpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dfab9d4d6..fb644a8db 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -367,7 +367,7 @@ static orb_advert_t status_pub; transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; - + // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); @@ -376,7 +376,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armed } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); } - + return arming_res; } @@ -1164,7 +1164,7 @@ 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.condition_landed) && - sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ @@ -1182,7 +1182,7 @@ 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 && - sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) { + sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp index 41f3ca0aa..0776894fb 100644 --- a/src/modules/commander/rc_calibration.cpp +++ b/src/modules/commander/rc_calibration.cpp @@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; + float p = sp.y; param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; + p = sp.x; param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; + p = sp.r; param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ -- cgit v1.2.3 From e5508a1aa07fb4a0e76dfd077c5beaa114f21695 Mon Sep 17 00:00:00 2001 From: Ash Charles Date: Tue, 13 May 2014 09:41:41 -0700 Subject: Add Gumstix AeroCore device Based on the work of Andrew Smith [1], add board configuration and device drivers to support the Gumstix AeroCore (previously Aerodroid) board [2]. The AeroCore is an autopilot board based on a STM32F427 similar to the FMUv2. [1] https://github.com/smithandrewc/Firmware [2] https://store.gumstix.com/index.php/products/585/ Signed-off-by: Ash Charles --- Images/aerocore.prototype | 12 + makefiles/board_aerocore.mk | 11 + makefiles/config_aerocore_default.mk | 125 +++ nuttx-configs/aerocore/include/board.h | 263 +++++++ nuttx-configs/aerocore/include/nsh_romfsimg.h | 42 + nuttx-configs/aerocore/nsh/Make.defs | 179 +++++ nuttx-configs/aerocore/nsh/appconfig | 42 + nuttx-configs/aerocore/nsh/defconfig | 950 +++++++++++++++++++++++ nuttx-configs/aerocore/nsh/setenv.sh | 67 ++ nuttx-configs/aerocore/scripts/ld.script | 150 ++++ nuttx-configs/aerocore/src/Makefile | 84 ++ nuttx-configs/aerocore/src/empty.c | 4 + src/drivers/boards/aerocore/aerocore_init.c | 282 +++++++ src/drivers/boards/aerocore/aerocore_led.c | 121 +++ src/drivers/boards/aerocore/aerocore_pwm_servo.c | 117 +++ src/drivers/boards/aerocore/aerocore_spi.c | 183 +++++ src/drivers/boards/aerocore/board_config.h | 176 +++++ src/drivers/boards/aerocore/module.mk | 8 + src/drivers/boards/px4fmu-v1/board_config.h | 2 + src/drivers/boards/px4fmu-v2/board_config.h | 2 + src/drivers/drv_gpio.h | 10 +- src/drivers/drv_gps.h | 4 + src/drivers/gps/gps.cpp | 11 +- src/drivers/lsm303d/lsm303d.cpp | 2 +- src/drivers/ms5611/ms5611_spi.cpp | 2 +- src/drivers/px4fmu/fmu.cpp | 84 +- src/drivers/stm32/adc/adc.cpp | 4 + src/drivers/stm32/drv_hrt.c | 4 +- src/lib/version/version.h | 4 + src/modules/commander/commander_helper.cpp | 2 +- src/modules/sensors/sensor_params.c | 9 + src/modules/sensors/sensors.cpp | 10 +- src/systemcmds/mtd/mtd.c | 4 + 33 files changed, 2960 insertions(+), 10 deletions(-) create mode 100644 Images/aerocore.prototype create mode 100644 makefiles/board_aerocore.mk create mode 100644 makefiles/config_aerocore_default.mk create mode 100644 nuttx-configs/aerocore/include/board.h create mode 100644 nuttx-configs/aerocore/include/nsh_romfsimg.h create mode 100644 nuttx-configs/aerocore/nsh/Make.defs create mode 100644 nuttx-configs/aerocore/nsh/appconfig create mode 100644 nuttx-configs/aerocore/nsh/defconfig create mode 100755 nuttx-configs/aerocore/nsh/setenv.sh create mode 100644 nuttx-configs/aerocore/scripts/ld.script create mode 100644 nuttx-configs/aerocore/src/Makefile create mode 100644 nuttx-configs/aerocore/src/empty.c create mode 100644 src/drivers/boards/aerocore/aerocore_init.c create mode 100644 src/drivers/boards/aerocore/aerocore_led.c create mode 100644 src/drivers/boards/aerocore/aerocore_pwm_servo.c create mode 100644 src/drivers/boards/aerocore/aerocore_spi.c create mode 100644 src/drivers/boards/aerocore/board_config.h create mode 100644 src/drivers/boards/aerocore/module.mk (limited to 'src/modules/commander') diff --git a/Images/aerocore.prototype b/Images/aerocore.prototype new file mode 100644 index 000000000..8502a0864 --- /dev/null +++ b/Images/aerocore.prototype @@ -0,0 +1,12 @@ +{ + "board_id": 19, + "magic": "AeroCore", + "description": "Firmware for the Gumstix AeroCore board", + "image": "", + "build_time": 0, + "summary": "AEROCORE", + "version": "0.1", + "image_size": 0, + "git_identity": "", + "board_revision": 0 +} diff --git a/makefiles/board_aerocore.mk b/makefiles/board_aerocore.mk new file mode 100644 index 000000000..6f4b93266 --- /dev/null +++ b/makefiles/board_aerocore.mk @@ -0,0 +1,11 @@ +# +# Board-specific definitions for the Gumstix AeroCore +# + +# +# Configure the toolchain +# +CONFIG_ARCH = CORTEXM4F +CONFIG_BOARD = AEROCORE + +include $(PX4_MK_DIR)/toolchain_gnu-arm-eabi.mk diff --git a/makefiles/config_aerocore_default.mk b/makefiles/config_aerocore_default.mk new file mode 100644 index 000000000..e038500a6 --- /dev/null +++ b/makefiles/config_aerocore_default.mk @@ -0,0 +1,125 @@ +# +# Makefile for the AeroCore *default* configuration +# + +# +# Use the configuration's ROMFS. +# +ROMFS_ROOT = $(PX4_BASE)/ROMFS/px4fmu_common + +# +# Board support modules +# +MODULES += drivers/device +MODULES += drivers/stm32 +MODULES += drivers/stm32/adc +MODULES += drivers/stm32/tone_alarm +MODULES += drivers/led +MODULES += drivers/px4fmu +MODULES += drivers/boards/aerocore +MODULES += drivers/lsm303d +MODULES += drivers/l3gd20 +MODULES += drivers/ms5611 +MODULES += drivers/gps +MODULES += drivers/hil +MODULES += modules/sensors + +# +# System commands +# +MODULES += systemcmds/boardinfo +MODULES += systemcmds/mixer +MODULES += systemcmds/param +MODULES += systemcmds/perf +MODULES += systemcmds/preflight_check +MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib +MODULES += systemcmds/reboot +MODULES += systemcmds/top +MODULES += systemcmds/config +MODULES += systemcmds/nshterm +MODULES += systemcmds/mtd +MODULES += systemcmds/dumpfile + +# +# General system control +# +MODULES += modules/commander +MODULES += modules/navigator +MODULES += modules/mavlink + +# +# Estimation modules (EKF/ SO3 / other filters) +# +MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_so3 +MODULES += modules/fw_att_pos_estimator +MODULES += modules/position_estimator_inav + +# +# Vehicle Control +# +MODULES += modules/fw_pos_control_l1 +MODULES += modules/fw_att_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control + +# +# Library modules +# +MODULES += modules/systemlib +MODULES += modules/systemlib/mixer +MODULES += modules/controllib +MODULES += modules/uORB +MODULES += modules/dataman + +# +# Libraries +# +LIBRARIES += lib/mathlib/CMSIS +MODULES += lib/mathlib +MODULES += lib/mathlib/math/filter +MODULES += lib/ecl +MODULES += lib/external_lgpl +MODULES += lib/geo +MODULES += lib/conversion +MODULES += lib/launchdetection + +# +# Demo apps +# +#MODULES += examples/math_demo +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/hello_sky +MODULES += examples/px4_simple_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/daemon +#MODULES += examples/px4_daemon_app + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/debug_values +#MODULES += examples/px4_mavlink_debug + +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + +# +# Transitional support - add commands from the NuttX export archive. +# +# In general, these should move to modules over time. +# +# Each entry here is ... but we use a helper macro +# to make the table a bit more readable. +# +define _B + $(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4) +endef + +BUILTIN_COMMANDS := \ + $(call _B, hello, , 2048, hello_main) \ + $(call _B, i2c, , 2048, i2c_main) diff --git a/nuttx-configs/aerocore/include/board.h b/nuttx-configs/aerocore/include/board.h new file mode 100644 index 000000000..8705c1bc2 --- /dev/null +++ b/nuttx-configs/aerocore/include/board.h @@ -0,0 +1,263 @@ +/************************************************************************************ + * configs/aerocore/include/board.h + * include/arch/board/board.h + * + * Copyright (C) 2009 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ************************************************************************************/ + +#ifndef __ARCH_BOARD_BOARD_H +#define __ARCH_BOARD_BOARD_H + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include +#ifndef __ASSEMBLY__ +# include +#endif + +#include + +/************************************************************************************ + * Definitions + ************************************************************************************/ + +/* Clocking *************************************************************************/ +/* The AeroCore uses a 24MHz crystal connected to the HSE. + * + * This is the "standard" configuration as set up by arch/arm/src/stm32f40xx_rcc.c: + * System Clock source : PLL (HSE) + * SYSCLK(Hz) : 168000000 Determined by PLL configuration + * HCLK(Hz) : 168000000 (STM32_RCC_CFGR_HPRE) + * AHB Prescaler : 1 (STM32_RCC_CFGR_HPRE) + * APB1 Prescaler : 4 (STM32_RCC_CFGR_PPRE1) + * APB2 Prescaler : 2 (STM32_RCC_CFGR_PPRE2) + * HSE Frequency(Hz) : 24000000 (STM32_BOARD_XTAL) + * PLLM : 24 (STM32_PLLCFG_PLLM) + * PLLN : 336 (STM32_PLLCFG_PLLN) + * PLLP : 2 (STM32_PLLCFG_PLLP) + * PLLQ : 7 (STM32_PLLCFG_PPQ) + * Main regulator output voltage : Scale1 mode Needed for high speed SYSCLK + * Flash Latency(WS) : 5 + * Prefetch Buffer : OFF + * Instruction cache : ON + * Data cache : ON + * Require 48MHz for USB OTG FS, : Enabled + * SDIO and RNG clock + */ + +/* HSI - 16 MHz RC factory-trimmed + * LSI - 32 KHz RC + * HSE - On-board crystal frequency is 24MHz + * LSE - not installed + */ + +#define STM32_BOARD_XTAL 24000000ul + +#define STM32_HSI_FREQUENCY 16000000ul +#define STM32_LSI_FREQUENCY 32000 +#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL + +/* Main PLL Configuration. + * + * PLL source is HSE + * PLL_VCO = (STM32_HSE_FREQUENCY / PLLM) * PLLN + * = (24,000,000 / 24) * 336 + * = 336,000,000 + * SYSCLK = PLL_VCO / PLLP + * = 336,000,000 / 2 = 168,000,000 + * USB OTG FS, SDIO and RNG Clock + * = PLL_VCO / PLLQ + * = 48,000,000 + */ + +#define STM32_PLLCFG_PLLM RCC_PLLCFG_PLLM(24) +#define STM32_PLLCFG_PLLN RCC_PLLCFG_PLLN(336) +#define STM32_PLLCFG_PLLP RCC_PLLCFG_PLLP_2 +#define STM32_PLLCFG_PLLQ RCC_PLLCFG_PLLQ(7) + +#define STM32_SYSCLK_FREQUENCY 168000000ul + +/* AHB clock (HCLK) is SYSCLK (168MHz) */ + +#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK /* HCLK = SYSCLK / 1 */ +#define STM32_HCLK_FREQUENCY STM32_SYSCLK_FREQUENCY +#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */ + +/* APB1 clock (PCLK1) is HCLK/4 (42MHz) */ + +#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd4 /* PCLK1 = HCLK / 4 */ +#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/4) + +/* Timers driven from APB1 will be twice PCLK1 */ + +#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM12_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM13_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB1_TIM14_CLKIN (2*STM32_PCLK1_FREQUENCY) + +/* APB2 clock (PCLK2) is HCLK/2 (84MHz) */ + +#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLKd2 /* PCLK2 = HCLK / 2 */ +#define STM32_PCLK2_FREQUENCY (STM32_HCLK_FREQUENCY/2) + +/* Timers driven from APB2 will be twice PCLK2 */ + +#define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) + +/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx + * otherwise frequency is 2xAPBx. + * Note: TIM1,8 are on APB2, others on APB1 + */ + +#define STM32_TIM18_FREQUENCY (2*STM32_PCLK2_FREQUENCY) +#define STM32_TIM27_FREQUENCY (2*STM32_PCLK1_FREQUENCY) + +/* Alternate function pin selections ************************************************/ + +/* + * UARTs. + */ +/* USART1 on PB[6,7]: GPS */ +#define GPIO_USART1_RX GPIO_USART1_RX_2 +#define GPIO_USART1_TX GPIO_USART1_TX_2 + +/* USART2 on PD[5,6]: J5 Breakout */ +#define GPIO_USART2_RX GPIO_USART2_RX_2 +#define GPIO_USART2_TX GPIO_USART2_TX_2 +#define GPIO_USART2_CTS 0 // unused +#define GPIO_USART2_RTS 0 // unused + +/* USART3 on PD[8,9]: to DuoVero UART2 */ +#define GPIO_USART3_RX GPIO_USART3_RX_3 +#define GPIO_USART3_TX GPIO_USART3_TX_3 +#define GPIO_USART3_CTS 0 // unused +#define GPIO_USART3_RTS 0 // unused + +/* UART7 on PE[78]: J7 Breakout */ +#define GPIO_UART7_RX GPIO_UART7_RX_1 +#define GPIO_UART7_TX GPIO_UART7_TX_1 + +/* + * UART8 on PE[0-1]: System Console on Port C of USB (J7) + * No alternate pin config +*/ + +/* USART[1,6] require a RX DMA configuration */ +#define DMAMAP_USART1_RX DMAMAP_USART1_RX_2 +#define DMAMAP_USART6_RX DMAMAP_USART6_RX_2 + +/* + * I2C + * + * The optional _GPIO configurations allow the I2C driver to manually + * reset the bus to clear stuck slaves. They match the pin configuration, + * but are normally-high GPIOs. + */ + +/* PB[10-11]: I2C2 is broken out on J9 header */ +#define GPIO_I2C2_SCL GPIO_I2C2_SCL_1 +#define GPIO_I2C2_SDA GPIO_I2C2_SDA_1 +#define GPIO_I2C2_SCL_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN10) +#define GPIO_I2C2_SDA_GPIO (GPIO_OUTPUT|GPIO_OPENDRAIN|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN11) + +/* + * SPI + */ +/* PA[4-7] SPI1 broken out on J12 */ +#define GPIO_SPI1_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) /* should be GPIO_SPI1_NSS_2 but use as a GPIO */ +#define GPIO_SPI1_SCK (GPIO_SPI1_SCK_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MISO (GPIO_SPI1_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI1_MOSI (GPIO_SPI1_MOSI_1|GPIO_SPEED_50MHz) + +/* PB[12-15]: SPI2 connected to DuoVero SPI1 */ +#define GPIO_SPI2_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN9) /* should be GPIO_SPI2_NSS_2 but use as a GPIO */ +#define GPIO_SPI2_SCK (GPIO_SPI2_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MISO (GPIO_SPI2_MISO_1|GPIO_SPEED_50MHz) +#define GPIO_SPI2_MOSI (GPIO_SPI2_MOSI_1|GPIO_SPEED_50MHz) + +/* PC[10-12]: SPI3 connected to onboard sensors */ +#define GPIO_SPI3_SCK (GPIO_SPI3_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MISO (GPIO_SPI3_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI3_MOSI (GPIO_SPI3_MOSI_2|GPIO_SPEED_50MHz) + +/* PE[11-14]: SPI4 connected to FRAM */ +#define GPIO_SPI4_NSS (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN11) /* should be GPIO_SPI4_NSS_2 but use as a GPIO */ +#define GPIO_SPI4_SCK (GPIO_SPI4_SCK_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MISO (GPIO_SPI4_MISO_2|GPIO_SPEED_50MHz) +#define GPIO_SPI4_MOSI (GPIO_SPI4_MOSI_2|GPIO_SPEED_50MHz) + +/************************************************************************************ + * Public Data + ************************************************************************************/ +#ifndef __ASSEMBLY__ + +#undef EXTERN +#if defined(__cplusplus) +#define EXTERN extern "C" +extern "C" { +#else +#define EXTERN extern +#endif + +/************************************************************************************ + * Public Function Prototypes + ************************************************************************************/ +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +EXTERN void stm32_boardinitialize(void); + +#undef EXTERN +#if defined(__cplusplus) +} +#endif + +#endif /* __ASSEMBLY__ */ +#endif /* __ARCH_BOARD_BOARD_H */ diff --git a/nuttx-configs/aerocore/include/nsh_romfsimg.h b/nuttx-configs/aerocore/include/nsh_romfsimg.h new file mode 100644 index 000000000..15e4e7a8d --- /dev/null +++ b/nuttx-configs/aerocore/include/nsh_romfsimg.h @@ -0,0 +1,42 @@ +/**************************************************************************** + * + * 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. + * + ****************************************************************************/ + +/** + * nsh_romfsetc.h + * + * This file is a stub for 'make export' purposes; the actual ROMFS + * must be supplied by the library client. + */ + +extern unsigned char romfs_img[]; +extern unsigned int romfs_img_len; diff --git a/nuttx-configs/aerocore/nsh/Make.defs b/nuttx-configs/aerocore/nsh/Make.defs new file mode 100644 index 000000000..c7a1b71bb --- /dev/null +++ b/nuttx-configs/aerocore/nsh/Make.defs @@ -0,0 +1,179 @@ +############################################################################ +# configs/aerocore/nsh/Make.defs +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +include ${TOPDIR}/.config +include ${TOPDIR}/tools/Config.mk + +# +# We only support building with the ARM bare-metal toolchain from +# https://launchpad.net/gcc-arm-embedded on Windows, Linux or Mac OS. +# +CONFIG_ARMV7M_TOOLCHAIN := GNU_EABI + +include ${TOPDIR}/arch/arm/src/armv7-m/Toolchain.defs + +CC = $(CROSSDEV)gcc +CXX = $(CROSSDEV)g++ +CPP = $(CROSSDEV)gcc -E +LD = $(CROSSDEV)ld +AR = $(CROSSDEV)ar rcs +NM = $(CROSSDEV)nm +OBJCOPY = $(CROSSDEV)objcopy +OBJDUMP = $(CROSSDEV)objdump + +MAXOPTIMIZATION = -O3 +ARCHCPUFLAGS = -mcpu=cortex-m4 \ + -mthumb \ + -march=armv7e-m \ + -mfpu=fpv4-sp-d16 \ + -mfloat-abi=hard + + +# enable precise stack overflow tracking +INSTRUMENTATIONDEFINES = -finstrument-functions \ + -ffixed-r10 + +# pull in *just* libm from the toolchain ... this is grody +LIBM = "${shell $(CC) $(ARCHCPUFLAGS) -print-file-name=libm.a}" +EXTRA_LIBS += $(LIBM) + +# use our linker script +LDSCRIPT = ld.script + +ifeq ($(WINTOOL),y) + # Windows-native toolchains + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" + ARCHXXINCLUDES = -I. -isystem "${shell cygpath -w $(TOPDIR)/include}" -isystem "${shell cygpath -w $(TOPDIR)/include/cxx}" + ARCHSCRIPT = -T "${shell cygpath -w $(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT)}" +else + ifeq ($(PX4_WINTOOL),y) + # Windows-native toolchains (MSYS) + DIRLINK = $(TOPDIR)/tools/copydir.sh + DIRUNLINK = $(TOPDIR)/tools/unlink.sh + MKDEP = $(TOPDIR)/tools/mknulldeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + else + # Linux/Cygwin-native toolchain + MKDEP = $(TOPDIR)/tools/mkdeps.sh + ARCHINCLUDES = -I. -isystem $(TOPDIR)/include + ARCHXXINCLUDES = -I. -isystem $(TOPDIR)/include -isystem $(TOPDIR)/include/cxx + ARCHSCRIPT = -T$(TOPDIR)/configs/$(CONFIG_ARCH_BOARD)/scripts/$(LDSCRIPT) + endif +endif + +# tool versions +ARCHCCVERSION = ${shell $(CC) -v 2>&1 | sed -n '/^gcc version/p' | sed -e 's/^gcc version \([0-9\.]\)/\1/g' -e 's/[-\ ].*//g' -e '1q'} +ARCHCCMAJOR = ${shell echo $(ARCHCCVERSION) | cut -d'.' -f1} + +# optimisation flags +ARCHOPTIMIZATION = $(MAXOPTIMIZATION) \ + -fno-strict-aliasing \ + -fno-strength-reduce \ + -fomit-frame-pointer \ + -funsafe-math-optimizations \ + -fno-builtin-printf \ + -ffunction-sections \ + -fdata-sections + +ifeq ("${CONFIG_DEBUG_SYMBOLS}","y") +ARCHOPTIMIZATION += -g +endif + +ARCHCFLAGS = -std=gnu99 +ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x +ARCHWARNINGS = -Wall \ + -Wextra \ + -Wdouble-promotion \ + -Wshadow \ + -Wfloat-equal \ + -Wframe-larger-than=1024 \ + -Wpointer-arith \ + -Wlogical-op \ + -Wmissing-declarations \ + -Wpacked \ + -Wno-unused-parameter +# -Wcast-qual - generates spurious noreturn attribute warnings, try again later +# -Wconversion - would be nice, but too many "risky-but-safe" conversions in the code +# -Wcast-align - would help catch bad casts in some cases, but generates too many false positives + +ARCHCWARNINGS = $(ARCHWARNINGS) \ + -Wbad-function-cast \ + -Wstrict-prototypes \ + -Wold-style-declaration \ + -Wmissing-parameter-type \ + -Wmissing-prototypes \ + -Wnested-externs \ + -Wunsuffixed-float-constants +ARCHWARNINGSXX = $(ARCHWARNINGS) \ + -Wno-psabi +ARCHDEFINES = +ARCHPICFLAGS = -fpic -msingle-pic-base -mpic-register=r10 + +# this seems to be the only way to add linker flags +EXTRA_LIBS += --warn-common \ + --gc-sections + +CFLAGS = $(ARCHCFLAGS) $(ARCHCWARNINGS) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe -fno-common +CPICFLAGS = $(ARCHPICFLAGS) $(CFLAGS) +CXXFLAGS = $(ARCHCXXFLAGS) $(ARCHWARNINGSXX) $(ARCHOPTIMIZATION) $(ARCHCPUFLAGS) $(ARCHXXINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) -pipe +CXXPICFLAGS = $(ARCHPICFLAGS) $(CXXFLAGS) +CPPFLAGS = $(ARCHINCLUDES) $(INSTRUMENTATIONDEFINES) $(ARCHDEFINES) $(EXTRADEFINES) +AFLAGS = $(CFLAGS) -D__ASSEMBLY__ + +NXFLATLDFLAGS1 = -r -d -warn-common +NXFLATLDFLAGS2 = $(NXFLATLDFLAGS1) -T$(TOPDIR)/binfmt/libnxflat/gnu-nxflat.ld -no-check-sections +LDNXFLATFLAGS = -e main -s 2048 + +OBJEXT = .o +LIBEXT = .a +EXEEXT = + + +# produce partially-linked $1 from files in $2 +define PRELINK + @echo "PRELINK: $1" + $(Q) $(LD) -Ur -o $1 $2 && $(OBJCOPY) --localize-hidden $1 +endef + +HOSTCC = gcc +HOSTINCLUDES = -I. +HOSTCFLAGS = -Wall -Wstrict-prototypes -Wshadow -g -pipe +HOSTLDFLAGS = + diff --git a/nuttx-configs/aerocore/nsh/appconfig b/nuttx-configs/aerocore/nsh/appconfig new file mode 100644 index 000000000..2850dac06 --- /dev/null +++ b/nuttx-configs/aerocore/nsh/appconfig @@ -0,0 +1,42 @@ +############################################################################ +# configs/aerocore/nsh/appconfig +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# +############################################################################ + +# Path to example in apps/examples containing the user_start entry point + +CONFIGURED_APPS += examples/nsh + +# The NSH application library +CONFIGURED_APPS += nshlib +CONFIGURED_APPS += system/readline diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig new file mode 100644 index 000000000..8d0bae7b9 --- /dev/null +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -0,0 +1,950 @@ +# +# Automatically generated file; DO NOT EDIT. +# Nuttx/ Configuration +# +CONFIG_NUTTX_NEWCONFIG=y + +# +# Build Setup +# +# CONFIG_EXPERIMENTAL is not set +CONFIG_HOST_LINUX=y +# CONFIG_HOST_OSX is not set +# CONFIG_HOST_WINDOWS is not set +# CONFIG_HOST_OTHER is not set + +# +# Build Configuration +# +CONFIG_APPS_DIR="../apps" +# CONFIG_BUILD_2PASS is not set + +# +# Binary Output Formats +# +# CONFIG_RRLOAD_BINARY is not set +# CONFIG_INTELHEX_BINARY is not set +# CONFIG_MOTOROLA_SREC is not set +CONFIG_RAW_BINARY=y + +# +# Customize Header Files +# +# CONFIG_ARCH_STDBOOL_H is not set +CONFIG_ARCH_MATH_H=y +# CONFIG_ARCH_FLOAT_H is not set +# CONFIG_ARCH_STDARG_H is not set + +# +# Debug Options +# +# CONFIG_DEBUG is not set +# CONFIG_DEBUG_SYMBOLS is not set + +# +# System Type +# +# CONFIG_ARCH_8051 is not set +CONFIG_ARCH_ARM=y +# CONFIG_ARCH_AVR is not set +# CONFIG_ARCH_HC is not set +# CONFIG_ARCH_MIPS is not set +# CONFIG_ARCH_RGMP is not set +# CONFIG_ARCH_SH is not set +# CONFIG_ARCH_SIM is not set +# CONFIG_ARCH_X86 is not set +# CONFIG_ARCH_Z16 is not set +# CONFIG_ARCH_Z80 is not set +CONFIG_ARCH="arm" + +# +# ARM Options +# +# CONFIG_ARCH_CHIP_C5471 is not set +# CONFIG_ARCH_CHIP_CALYPSO is not set +# CONFIG_ARCH_CHIP_DM320 is not set +# CONFIG_ARCH_CHIP_IMX is not set +# CONFIG_ARCH_CHIP_KINETIS is not set +# CONFIG_ARCH_CHIP_KL is not set +# CONFIG_ARCH_CHIP_LM is not set +# CONFIG_ARCH_CHIP_LPC17XX is not set +# CONFIG_ARCH_CHIP_LPC214X is not set +# CONFIG_ARCH_CHIP_LPC2378 is not set +# CONFIG_ARCH_CHIP_LPC31XX is not set +# CONFIG_ARCH_CHIP_LPC43XX is not set +# CONFIG_ARCH_CHIP_NUC1XX is not set +# CONFIG_ARCH_CHIP_SAM34 is not set +CONFIG_ARCH_CHIP_STM32=y +# CONFIG_ARCH_CHIP_STR71X is not set +CONFIG_ARCH_CORTEXM4=y +CONFIG_ARCH_FAMILY="armv7-m" +CONFIG_ARCH_CHIP="stm32" +CONFIG_ARMV7M_USEBASEPRI=y +CONFIG_ARCH_HAVE_CMNVECTOR=y +CONFIG_ARMV7M_CMNVECTOR=y +CONFIG_ARCH_HAVE_FPU=y +CONFIG_ARCH_FPU=y +CONFIG_ARCH_HAVE_MPU=y +# CONFIG_ARMV7M_MPU is not set + +# +# ARMV7M Configuration Options +# +# CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODEREDL is not set +# CONFIG_ARMV7M_TOOLCHAIN_CODESOURCERYL is not set +CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y +CONFIG_ARMV7M_STACKCHECK=y +CONFIG_SERIAL_TERMIOS=y + +# +# STM32 Configuration Options +# +# CONFIG_ARCH_CHIP_STM32L151C6 is not set +# CONFIG_ARCH_CHIP_STM32L151C8 is not set +# CONFIG_ARCH_CHIP_STM32L151CB is not set +# CONFIG_ARCH_CHIP_STM32L151R6 is not set +# CONFIG_ARCH_CHIP_STM32L151R8 is not set +# CONFIG_ARCH_CHIP_STM32L151RB is not set +# CONFIG_ARCH_CHIP_STM32L151V6 is not set +# CONFIG_ARCH_CHIP_STM32L151V8 is not set +# CONFIG_ARCH_CHIP_STM32L151VB is not set +# CONFIG_ARCH_CHIP_STM32L152C6 is not set +# CONFIG_ARCH_CHIP_STM32L152C8 is not set +# CONFIG_ARCH_CHIP_STM32L152CB is not set +# CONFIG_ARCH_CHIP_STM32L152R6 is not set +# CONFIG_ARCH_CHIP_STM32L152R8 is not set +# CONFIG_ARCH_CHIP_STM32L152RB is not set +# CONFIG_ARCH_CHIP_STM32L152V6 is not set +# CONFIG_ARCH_CHIP_STM32L152V8 is not set +# CONFIG_ARCH_CHIP_STM32L152VB is not set +# CONFIG_ARCH_CHIP_STM32F100C8 is not set +# CONFIG_ARCH_CHIP_STM32F100CB is not set +# CONFIG_ARCH_CHIP_STM32F100R8 is not set +# CONFIG_ARCH_CHIP_STM32F100RB is not set +# CONFIG_ARCH_CHIP_STM32F100RC is not set +# CONFIG_ARCH_CHIP_STM32F100RD is not set +# CONFIG_ARCH_CHIP_STM32F100RE is not set +# CONFIG_ARCH_CHIP_STM32F100V8 is not set +# CONFIG_ARCH_CHIP_STM32F100VB is not set +# CONFIG_ARCH_CHIP_STM32F100VC is not set +# CONFIG_ARCH_CHIP_STM32F100VD is not set +# CONFIG_ARCH_CHIP_STM32F100VE is not set +# CONFIG_ARCH_CHIP_STM32F103C4 is not set +# CONFIG_ARCH_CHIP_STM32F103C8 is not set +# CONFIG_ARCH_CHIP_STM32F103RET6 is not set +# CONFIG_ARCH_CHIP_STM32F103VCT6 is not set +# CONFIG_ARCH_CHIP_STM32F103VET6 is not set +# CONFIG_ARCH_CHIP_STM32F103ZET6 is not set +# CONFIG_ARCH_CHIP_STM32F105VBT7 is not set +# CONFIG_ARCH_CHIP_STM32F107VC is not set +# CONFIG_ARCH_CHIP_STM32F207IG is not set +# CONFIG_ARCH_CHIP_STM32F302CB is not set +# CONFIG_ARCH_CHIP_STM32F302CC is not set +# CONFIG_ARCH_CHIP_STM32F302RB is not set +# CONFIG_ARCH_CHIP_STM32F302RC is not set +# CONFIG_ARCH_CHIP_STM32F302VB is not set +# CONFIG_ARCH_CHIP_STM32F302VC is not set +# CONFIG_ARCH_CHIP_STM32F303CB is not set +# CONFIG_ARCH_CHIP_STM32F303CC is not set +# CONFIG_ARCH_CHIP_STM32F303RB is not set +# CONFIG_ARCH_CHIP_STM32F303RC is not set +# CONFIG_ARCH_CHIP_STM32F303VB is not set +# CONFIG_ARCH_CHIP_STM32F303VC is not set +# CONFIG_ARCH_CHIP_STM32F405RG is not set +# CONFIG_ARCH_CHIP_STM32F405VG is not set +# CONFIG_ARCH_CHIP_STM32F405ZG is not set +# CONFIG_ARCH_CHIP_STM32F407VE is not set +# CONFIG_ARCH_CHIP_STM32F407VG is not set +# CONFIG_ARCH_CHIP_STM32F407ZE is not set +# CONFIG_ARCH_CHIP_STM32F407ZG is not set +# CONFIG_ARCH_CHIP_STM32F407IE is not set +# CONFIG_ARCH_CHIP_STM32F407IG is not set +CONFIG_ARCH_CHIP_STM32F427V=y +# CONFIG_ARCH_CHIP_STM32F427Z is not set +# CONFIG_ARCH_CHIP_STM32F427I is not set +# CONFIG_STM32_STM32L15XX is not set +# CONFIG_STM32_ENERGYLITE is not set +# CONFIG_STM32_STM32F10XX is not set +# CONFIG_STM32_VALUELINE is not set +# CONFIG_STM32_CONNECTIVITYLINE is not set +# CONFIG_STM32_PERFORMANCELINE is not set +# CONFIG_STM32_HIGHDENSITY is not set +# CONFIG_STM32_MEDIUMDENSITY is not set +# CONFIG_STM32_LOWDENSITY is not set +# CONFIG_STM32_STM32F20XX is not set +# CONFIG_STM32_STM32F30XX is not set +CONFIG_STM32_STM32F40XX=y +CONFIG_STM32_STM32F427=y +# CONFIG_STM32_DFU is not set + +# +# STM32 Peripheral Support +# +CONFIG_STM32_ADC1=y +# CONFIG_STM32_ADC2 is not set +# CONFIG_STM32_ADC3 is not set +CONFIG_STM32_BKPSRAM=y +# CONFIG_STM32_CAN1 is not set +# CONFIG_STM32_CAN2 is not set +CONFIG_STM32_CCMDATARAM=y +# CONFIG_STM32_CRC is not set +# CONFIG_STM32_CRYP is not set +CONFIG_STM32_DMA1=y +CONFIG_STM32_DMA2=y +# CONFIG_STM32_DAC1 is not set +# CONFIG_STM32_DAC2 is not set +# CONFIG_STM32_DCMI is not set +# CONFIG_STM32_ETHMAC is not set +# CONFIG_STM32_FSMC is not set +# CONFIG_STM32_HASH is not set +# CONFIG_STM32_I2C1 is not set +CONFIG_STM32_I2C2=y +# CONFIG_STM32_I2C3 is not set +# CONFIG_STM32_OTGFS is not set +# CONFIG_STM32_OTGHS is not set +CONFIG_STM32_PWR=y +# CONFIG_STM32_RNG is not set +# CONFIG_STM32_SDIO is not set +CONFIG_STM32_SPI1=y +CONFIG_STM32_SPI2=y +CONFIG_STM32_SPI3=y +CONFIG_STM32_SPI4=y +# CONFIG_STM32_SPI5 is not set +# CONFIG_STM32_SPI6 is not set +CONFIG_STM32_SYSCFG=y +CONFIG_STM32_TIM1=y +# CONFIG_STM32_TIM2 is not set +CONFIG_STM32_TIM3=y +CONFIG_STM32_TIM4=y +CONFIG_STM32_TIM5=y +# CONFIG_STM32_TIM6 is not set +# CONFIG_STM32_TIM7 is not set +# CONFIG_STM32_TIM8 is not set +CONFIG_STM32_TIM9=y +# CONFIG_STM32_TIM10 is not set +# CONFIG_STM32_TIM11 is not set +# CONFIG_STM32_TIM12 is not set +# CONFIG_STM32_TIM13 is not set +# CONFIG_STM32_TIM14 is not set +CONFIG_STM32_USART1=y +CONFIG_STM32_USART2=y +CONFIG_STM32_USART3=y +# CONFIG_STM32_UART4 is not set +# CONFIG_STM32_UART5 is not set +# CONFIG_STM32_USART6 is not set +CONFIG_STM32_UART7=y +CONFIG_STM32_UART8=y +# CONFIG_STM32_IWDG is not set +CONFIG_STM32_WWDG=y +CONFIG_STM32_ADC=y +CONFIG_STM32_SPI=y +CONFIG_STM32_I2C=y + +# +# Alternate Pin Mapping +# +CONFIG_STM32_FLASH_PREFETCH=y +# CONFIG_STM32_JTAG_DISABLE is not set +# CONFIG_STM32_JTAG_FULL_ENABLE is not set +# CONFIG_STM32_JTAG_NOJNTRST_ENABLE is not set +CONFIG_STM32_JTAG_SW_ENABLE=y +CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y +# CONFIG_STM32_FORCEPOWER is not set +# CONFIG_ARCH_BOARD_STM32_CUSTOM_CLOCKCONFIG is not set +# CONFIG_STM32_CCMEXCLUDE is not set +CONFIG_STM32_DMACAPABLE=y +# CONFIG_STM32_TIM1_PWM is not set +# CONFIG_STM32_TIM3_PWM is not set +# CONFIG_STM32_TIM4_PWM is not set +# CONFIG_STM32_TIM5_PWM is not set +# CONFIG_STM32_TIM9_PWM is not set +# CONFIG_STM32_TIM1_ADC is not set +# CONFIG_STM32_TIM3_ADC is not set +# CONFIG_STM32_TIM4_ADC is not set +# CONFIG_STM32_TIM5_ADC is not set +CONFIG_STM32_USART=y + +# +# U[S]ART Configuration +# +# CONFIG_USART1_RS485 is not set +CONFIG_USART1_RXDMA=y +# CONFIG_USART2_RS485 is not set +CONFIG_USART2_RXDMA=y +# CONFIG_USART3_RS485 is not set +CONFIG_USART3_RXDMA=y +# CONFIG_UART7_RS485 is not set +CONFIG_UART7_RXDMA=y +# CONFIG_UART8_RS485 is not set +CONFIG_UART8_RXDMA=y +CONFIG_SERIAL_DISABLE_REORDERING=y +CONFIG_STM32_USART_SINGLEWIRE=y + +# +# SPI Configuration +# +# CONFIG_STM32_SPI_INTERRUPTS is not set +# CONFIG_STM32_SPI_DMA is not set + +# +# I2C Configuration +# +# CONFIG_STM32_I2C_DYNTIMEO is not set +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=10 +CONFIG_STM32_I2CTIMEOTICKS=500 +# CONFIG_STM32_I2C_DUTY16_9 is not set + +# +# USB Host Configuration +# + +# +# USB Device Configuration +# + +# +# External Memory Configuration +# + +# +# Architecture Options +# +# CONFIG_ARCH_NOINTC is not set +# CONFIG_ARCH_VECNOTIRQ is not set +CONFIG_ARCH_DMA=y +CONFIG_ARCH_IRQPRIO=y +# CONFIG_CUSTOM_STACK is not set +# CONFIG_ADDRENV is not set +CONFIG_ARCH_HAVE_VFORK=y +CONFIG_ARCH_STACKDUMP=y +# CONFIG_ENDIAN_BIG is not set +# CONFIG_ARCH_HAVE_RAMFUNCS is not set +CONFIG_ARCH_HAVE_RAMVECTORS=y +# CONFIG_ARCH_RAMVECTORS is not set + +# +# Board Settings +# +CONFIG_BOARD_LOOPSPERMSEC=16717 +# CONFIG_ARCH_CALIBRATION is not set +CONFIG_DRAM_START=0x20000000 +CONFIG_DRAM_SIZE=262144 +CONFIG_ARCH_HAVE_INTERRUPTSTACK=y +CONFIG_ARCH_INTERRUPTSTACK=4096 + +# +# Boot options +# +# CONFIG_BOOT_RUNFROMEXTSRAM is not set +CONFIG_BOOT_RUNFROMFLASH=y +# CONFIG_BOOT_RUNFROMISRAM is not set +# CONFIG_BOOT_RUNFROMSDRAM is not set +# CONFIG_BOOT_COPYTORAM is not set + +# +# Board Selection +# +CONFIG_ARCH_BOARD_CUSTOM=y +CONFIG_ARCH_BOARD="" + +# +# Common Board Options +# +CONFIG_NSH_MMCSDMINOR=0 + +# +# Board-Specific Options +# + +# +# RTOS Features +# +# CONFIG_BOARD_INITIALIZE is not set +CONFIG_MSEC_PER_TICK=1 +CONFIG_RR_INTERVAL=0 +CONFIG_SCHED_INSTRUMENTATION=y +CONFIG_TASK_NAME_SIZE=24 +# CONFIG_SCHED_HAVE_PARENT is not set +# CONFIG_JULIAN_TIME is not set +CONFIG_START_YEAR=1970 +CONFIG_START_MONTH=1 +CONFIG_START_DAY=1 +CONFIG_DEV_CONSOLE=y +# CONFIG_MUTEX_TYPES is not set +CONFIG_PRIORITY_INHERITANCE=y +CONFIG_SEM_PREALLOCHOLDERS=0 +CONFIG_SEM_NNESTPRIO=8 +# CONFIG_FDCLONE_DISABLE is not set +CONFIG_FDCLONE_STDIO=y +CONFIG_SDCLONE_DISABLE=y +CONFIG_SCHED_WAITPID=y +# CONFIG_SCHED_STARTHOOK is not set +CONFIG_SCHED_ATEXIT=y +CONFIG_SCHED_ATEXIT_MAX=1 +# CONFIG_SCHED_ONEXIT is not set +CONFIG_USER_ENTRYPOINT="nsh_main" +# CONFIG_DISABLE_OS_API is not set + +# +# Signal Numbers +# +CONFIG_SIG_SIGUSR1=1 +CONFIG_SIG_SIGUSR2=2 +CONFIG_SIG_SIGALARM=3 +CONFIG_SIG_SIGCONDTIMEDOUT=16 +CONFIG_SIG_SIGWORK=4 + +# +# Sizes of configurable things (0 disables) +# +CONFIG_MAX_TASKS=32 +CONFIG_MAX_TASK_ARGS=10 +CONFIG_NPTHREAD_KEYS=4 +CONFIG_NFILE_DESCRIPTORS=36 +CONFIG_NFILE_STREAMS=8 +CONFIG_NAME_MAX=32 +CONFIG_PREALLOC_MQ_MSGS=4 +CONFIG_MQ_MAXMSGSIZE=32 +CONFIG_MAX_WDOGPARMS=2 +CONFIG_PREALLOC_WDOGS=50 +CONFIG_PREALLOC_TIMERS=50 + +# +# Stack and heap information +# +CONFIG_IDLETHREAD_STACKSIZE=6000 +CONFIG_USERMAIN_STACKSIZE=4096 +CONFIG_PTHREAD_STACK_MIN=512 +CONFIG_PTHREAD_STACK_DEFAULT=2048 + +# +# Device Drivers +# +# CONFIG_DISABLE_POLL is not set +CONFIG_DEV_NULL=y +# CONFIG_DEV_ZERO is not set +# CONFIG_LOOP is not set +# CONFIG_RAMDISK is not set +# CONFIG_CAN is not set +# CONFIG_PWM is not set +CONFIG_I2C=y +# CONFIG_I2C_SLAVE is not set +CONFIG_I2C_TRANSFER=y +# CONFIG_I2C_WRITEREAD is not set +# CONFIG_I2C_POLLED is not set +# CONFIG_I2C_TRACE is not set +CONFIG_ARCH_HAVE_I2CRESET=y +CONFIG_I2C_RESET=y +CONFIG_SPI=y +# CONFIG_SPI_OWNBUS is not set +CONFIG_SPI_EXCHANGE=y +# CONFIG_SPI_CMDDATA is not set +# CONFIG_RTC is not set +CONFIG_WATCHDOG=y +# CONFIG_ANALOG is not set +# CONFIG_AUDIO_DEVICES is not set +# CONFIG_BCH is not set +# CONFIG_INPUT is not set +# CONFIG_LCD is not set +# CONFIG_MMCSD is not set +CONFIG_MTD=y + +# +# MTD Configuration +# +CONFIG_MTD_PARTITION=y +CONFIG_MTD_BYTE_WRITE=y + +# +# MTD Device Drivers +# +# CONFIG_RAMMTD is not set +# CONFIG_MTD_AT24XX is not set +# CONFIG_MTD_AT45DB is not set +# CONFIG_MTD_M25P is not set +# CONFIG_MTD_SMART is not set +CONFIG_MTD_RAMTRON=y +CONFIG_RAMTRON_FUJITSU=y +# CONFIG_MTD_SST25 is not set +# CONFIG_MTD_SST39FV is not set +# CONFIG_MTD_W25 is not set +CONFIG_PIPES=y +# CONFIG_PM is not set +# CONFIG_POWER is not set +# CONFIG_SENSORS is not set +CONFIG_SERIAL=y +# CONFIG_DEV_LOWCONSOLE is not set +# CONFIG_16550_UART is not set +CONFIG_ARCH_HAVE_UART7=y +CONFIG_ARCH_HAVE_UART8=y +CONFIG_ARCH_HAVE_USART1=y +CONFIG_ARCH_HAVE_USART2=y +CONFIG_ARCH_HAVE_USART3=y +CONFIG_MCU_SERIAL=y +CONFIG_STANDARD_SERIAL=y +CONFIG_SERIAL_NPOLLWAITERS=2 +# CONFIG_USART1_SERIAL_CONSOLE is not set +# CONFIG_USART2_SERIAL_CONSOLE is not set +# CONFIG_USART3_SERIAL_CONSOLE is not set +# CONFIG_UART7_SERIAL_CONSOLE is not set +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set + +# +# USART1 Configuration +# +CONFIG_USART1_RXBUFSIZE=512 +CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_BAUD=115200 +CONFIG_USART1_BITS=8 +CONFIG_USART1_PARITY=0 +CONFIG_USART1_2STOP=0 +# CONFIG_USART1_IFLOWCONTROL is not set +# CONFIG_USART1_OFLOWCONTROL is not set + +# +# USART2 Configuration +# +CONFIG_USART2_RXBUFSIZE=512 +CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_BAUD=115200 +CONFIG_USART2_BITS=8 +CONFIG_USART2_PARITY=0 +CONFIG_USART2_2STOP=0 +# CONFIG_USART2_IFLOWCONTROL is not set +# CONFIG_USART2_OFLOWCONTROL is not set + +# +# USART3 Configuration +# +CONFIG_USART3_RXBUFSIZE=512 +CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_BAUD=115200 +CONFIG_USART3_BITS=8 +CONFIG_USART3_PARITY=0 +CONFIG_USART3_2STOP=0 +# CONFIG_USART3_IFLOWCONTROL is not set +# CONFIG_USART3_OFLOWCONTROL is not set + +# +# UART7 Configuration +# +CONFIG_UART7_RXBUFSIZE=512 +CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_BAUD=115200 +CONFIG_UART7_BITS=8 +CONFIG_UART7_PARITY=0 +CONFIG_UART7_2STOP=0 +# CONFIG_UART7_IFLOWCONTROL is not set +# CONFIG_UART7_OFLOWCONTROL is not set + +# +# UART8 Configuration +# +CONFIG_UART8_RXBUFSIZE=512 +CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_BAUD=115200 +CONFIG_UART8_BITS=8 +CONFIG_UART8_PARITY=0 +CONFIG_UART8_2STOP=0 +# CONFIG_UART8_IFLOWCONTROL is not set +# CONFIG_UART8_OFLOWCONTROL is not set +# CONFIG_SERIAL_IFLOWCONTROL is not set +# CONFIG_SERIAL_OFLOWCONTROL is not set +# CONFIG_USBDEV is not set +# CONFIG_USBHOST is not set +# CONFIG_WIRELESS is not set + +# +# System Logging Device Options +# + +# +# System Logging +# +# CONFIG_RAMLOG is not set + +# +# Networking Support +# +# CONFIG_NET is not set + +# +# File Systems +# + +# +# File system configuration +# +# CONFIG_DISABLE_MOUNTPOINT is not set +# CONFIG_FS_RAMMAP is not set +CONFIG_FS_FAT=y +CONFIG_FAT_LCNAMES=y +CONFIG_FAT_LFN=y +CONFIG_FAT_MAXFNAME=32 +CONFIG_FS_FATTIME=y +CONFIG_FAT_DMAMEMORY=y +CONFIG_FS_NXFFS=y +CONFIG_NXFFS_PREALLOCATED=y +CONFIG_NXFFS_ERASEDSTATE=0xff +CONFIG_NXFFS_PACKTHRESHOLD=32 +CONFIG_NXFFS_MAXNAMLEN=32 +CONFIG_NXFFS_TAILTHRESHOLD=2048 +CONFIG_FS_ROMFS=y +# CONFIG_FS_SMARTFS is not set +CONFIG_FS_BINFS=y + +# +# System Logging +# +# CONFIG_SYSLOG_ENABLE is not set +# CONFIG_SYSLOG is not set + +# +# Graphics Support +# +# CONFIG_NX is not set + +# +# Memory Management +# +# CONFIG_MM_MULTIHEAP is not set +# CONFIG_MM_SMALL is not set +CONFIG_MM_REGIONS=2 +CONFIG_GRAN=y +# CONFIG_GRAN_SINGLE is not set +# CONFIG_GRAN_INTR is not set + +# +# Audio Support +# +# CONFIG_AUDIO is not set + +# +# Binary Formats +# +# CONFIG_BINFMT_DISABLE is not set +# CONFIG_BINFMT_EXEPATH is not set +# CONFIG_NXFLAT is not set +# CONFIG_ELF is not set +CONFIG_BUILTIN=y +# CONFIG_PIC is not set +# CONFIG_SYMTAB_ORDEREDBYNAME is not set + +# +# Library Routines +# + +# +# Standard C Library Options +# +CONFIG_STDIO_BUFFER_SIZE=32 +CONFIG_STDIO_LINEBUFFER=y +CONFIG_NUNGET_CHARS=2 +CONFIG_LIB_HOMEDIR="/" +# CONFIG_NOPRINTF_FIELDWIDTH is not set +CONFIG_LIBC_FLOATINGPOINT=y +CONFIG_LIB_RAND_ORDER=1 +# CONFIG_EOL_IS_CR is not set +# CONFIG_EOL_IS_LF is not set +# CONFIG_EOL_IS_BOTH_CRLF is not set +CONFIG_EOL_IS_EITHER_CRLF=y +# CONFIG_LIBC_EXECFUNCS is not set +CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=1024 +CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=2048 +CONFIG_LIBC_STRERROR=y +# CONFIG_LIBC_STRERROR_SHORT is not set +# CONFIG_LIBC_PERROR_STDOUT is not set +CONFIG_ARCH_LOWPUTC=y +CONFIG_LIB_SENDFILE_BUFSIZE=512 +# CONFIG_ARCH_ROMGETC is not set +CONFIG_ARCH_OPTIMIZED_FUNCTIONS=y +CONFIG_ARCH_MEMCPY=y +# CONFIG_ARCH_MEMCMP is not set +# CONFIG_ARCH_MEMMOVE is not set +# CONFIG_ARCH_MEMSET is not set +# CONFIG_MEMSET_OPTSPEED is not set +# CONFIG_ARCH_STRCHR is not set +# CONFIG_ARCH_STRCMP is not set +# CONFIG_ARCH_STRCPY is not set +# CONFIG_ARCH_STRNCPY is not set +# CONFIG_ARCH_STRLEN is not set +# CONFIG_ARCH_STRNLEN is not set +# CONFIG_ARCH_BZERO is not set + +# +# Non-standard Library Support +# +CONFIG_SCHED_WORKQUEUE=y +CONFIG_SCHED_HPWORK=y +CONFIG_SCHED_WORKPRIORITY=192 +CONFIG_SCHED_WORKPERIOD=5000 +CONFIG_SCHED_WORKSTACKSIZE=2000 +CONFIG_SCHED_LPWORK=y +CONFIG_SCHED_LPWORKPRIORITY=50 +CONFIG_SCHED_LPWORKPERIOD=50000 +CONFIG_SCHED_LPWORKSTACKSIZE=2000 +# CONFIG_LIB_KBDCODEC is not set +# CONFIG_LIB_SLCDCODEC is not set + +# +# Basic CXX Support +# +CONFIG_C99_BOOL8=y +CONFIG_HAVE_CXX=y +CONFIG_HAVE_CXXINITIALIZE=y +# CONFIG_CXX_NEWLONG is not set + +# +# uClibc++ Standard C++ Library +# +# CONFIG_UCLIBCXX is not set + +# +# Application Configuration +# + +# +# Built-In Applications +# +CONFIG_BUILTIN_PROXY_STACKSIZE=1024 + +# +# Examples +# +# CONFIG_EXAMPLES_BUTTONS is not set +# CONFIG_EXAMPLES_CAN is not set +# CONFIG_EXAMPLES_COMPOSITE is not set +# CONFIG_EXAMPLES_CXXTEST is not set +# CONFIG_EXAMPLES_DHCPD is not set +# CONFIG_EXAMPLES_ELF is not set +# CONFIG_EXAMPLES_FTPC is not set +# CONFIG_EXAMPLES_FTPD is not set +CONFIG_EXAMPLES_HELLO=y +# CONFIG_EXAMPLES_HELLOXX is not set +# CONFIG_EXAMPLES_JSON is not set +# CONFIG_EXAMPLES_HIDKBD is not set +# CONFIG_EXAMPLES_KEYPADTEST is not set +# CONFIG_EXAMPLES_IGMP is not set +# CONFIG_EXAMPLES_LCDRW is not set +# CONFIG_EXAMPLES_MM is not set +# CONFIG_EXAMPLES_MODBUS is not set +CONFIG_EXAMPLES_MOUNT=y +# CONFIG_EXAMPLES_MTDPART is not set +# CONFIG_EXAMPLES_NRF24L01TERM is not set +CONFIG_EXAMPLES_NSH=y +# CONFIG_EXAMPLES_NULL is not set +# CONFIG_EXAMPLES_NX is not set +# CONFIG_EXAMPLES_NXCONSOLE is not set +# CONFIG_EXAMPLES_NXFFS is not set +# CONFIG_EXAMPLES_NXFLAT is not set +# CONFIG_EXAMPLES_NXHELLO is not set +# CONFIG_EXAMPLES_NXIMAGE is not set +# CONFIG_EXAMPLES_NXLINES is not set +# CONFIG_EXAMPLES_NXTEXT is not set +# CONFIG_EXAMPLES_OSTEST is not set +# CONFIG_EXAMPLES_PASHELLO is not set +# CONFIG_EXAMPLES_PIPE is not set +# CONFIG_EXAMPLES_POSIXSPAWN is not set +# CONFIG_EXAMPLES_QENCODER is not set +# CONFIG_EXAMPLES_RGMP is not set +# CONFIG_EXAMPLES_ROMFS is not set +# CONFIG_EXAMPLES_SENDMAIL is not set +# CONFIG_EXAMPLES_SERLOOP is not set +# CONFIG_EXAMPLES_SLCD is not set +# CONFIG_EXAMPLES_SMART_TEST is not set +# CONFIG_EXAMPLES_SMART is not set +# CONFIG_EXAMPLES_TCPECHO is not set +# CONFIG_EXAMPLES_TELNETD is not set +# CONFIG_EXAMPLES_THTTPD is not set +# CONFIG_EXAMPLES_TIFF is not set +# CONFIG_EXAMPLES_TOUCHSCREEN is not set +# CONFIG_EXAMPLES_UDP is not set +# CONFIG_EXAMPLES_UIP is not set +# CONFIG_EXAMPLES_USBSERIAL is not set +# CONFIG_EXAMPLES_USBMSC is not set +# CONFIG_EXAMPLES_USBTERM is not set +# CONFIG_EXAMPLES_WATCHDOG is not set + +# +# Graphics Support +# +# CONFIG_TIFF is not set + +# +# Interpreters +# +# CONFIG_INTERPRETERS_FICL is not set +# CONFIG_INTERPRETERS_PCODE is not set + +# +# Network Utilities +# + +# +# Networking Utilities +# +# CONFIG_NETUTILS_CODECS is not set +# CONFIG_NETUTILS_DHCPC is not set +# CONFIG_NETUTILS_DHCPD is not set +# CONFIG_NETUTILS_FTPC is not set +# CONFIG_NETUTILS_FTPD is not set +# CONFIG_NETUTILS_JSON is not set +# CONFIG_NETUTILS_RESOLV is not set +# CONFIG_NETUTILS_SMTP is not set +# CONFIG_NETUTILS_TELNETD is not set +# CONFIG_NETUTILS_TFTPC is not set +# CONFIG_NETUTILS_THTTPD is not set +# CONFIG_NETUTILS_UIPLIB is not set +# CONFIG_NETUTILS_WEBCLIENT is not set + +# +# FreeModBus +# +# CONFIG_MODBUS is not set + +# +# NSH Library +# +CONFIG_NSH_LIBRARY=y +CONFIG_NSH_BUILTIN_APPS=y + +# +# Disable Individual commands +# +# CONFIG_NSH_DISABLE_CAT is not set +# CONFIG_NSH_DISABLE_CD is not set +# CONFIG_NSH_DISABLE_CP is not set +# CONFIG_NSH_DISABLE_DD is not set +# CONFIG_NSH_DISABLE_ECHO is not set +# CONFIG_NSH_DISABLE_EXEC is not set +# CONFIG_NSH_DISABLE_EXIT is not set +# CONFIG_NSH_DISABLE_FREE is not set +# CONFIG_NSH_DISABLE_GET is not set +# CONFIG_NSH_DISABLE_HELP is not set +# CONFIG_NSH_DISABLE_HEXDUMP is not set +# CONFIG_NSH_DISABLE_IFCONFIG is not set +# CONFIG_NSH_DISABLE_KILL is not set +# CONFIG_NSH_DISABLE_LOSETUP is not set +# CONFIG_NSH_DISABLE_LS is not set +# CONFIG_NSH_DISABLE_MB is not set +# CONFIG_NSH_DISABLE_MKDIR is not set +# CONFIG_NSH_DISABLE_MKFATFS is not set +# CONFIG_NSH_DISABLE_MKFIFO is not set +# CONFIG_NSH_DISABLE_MKRD is not set +# CONFIG_NSH_DISABLE_MH is not set +# CONFIG_NSH_DISABLE_MOUNT is not set +# CONFIG_NSH_DISABLE_MW is not set +# CONFIG_NSH_DISABLE_NSFMOUNT is not set +# CONFIG_NSH_DISABLE_PS is not set +# CONFIG_NSH_DISABLE_PING is not set +# CONFIG_NSH_DISABLE_PUT is not set +# CONFIG_NSH_DISABLE_PWD is not set +# CONFIG_NSH_DISABLE_RM is not set +# CONFIG_NSH_DISABLE_RMDIR is not set +# CONFIG_NSH_DISABLE_SET is not set +# CONFIG_NSH_DISABLE_SH is not set +# CONFIG_NSH_DISABLE_SLEEP is not set +# CONFIG_NSH_DISABLE_TEST is not set +# CONFIG_NSH_DISABLE_UMOUNT is not set +# CONFIG_NSH_DISABLE_UNSET is not set +# CONFIG_NSH_DISABLE_USLEEP is not set +# CONFIG_NSH_DISABLE_WGET is not set +# CONFIG_NSH_DISABLE_XD is not set + +# +# Configure Command Options +# +# CONFIG_NSH_CMDOPT_DF_H is not set +CONFIG_NSH_CODECS_BUFSIZE=128 +CONFIG_NSH_FILEIOSIZE=512 +CONFIG_NSH_STRERROR=y +CONFIG_NSH_LINELEN=128 +CONFIG_NSH_MAXARGUMENTS=12 +CONFIG_NSH_NESTDEPTH=8 +# CONFIG_NSH_DISABLESCRIPT is not set +# CONFIG_NSH_DISABLEBG is not set +CONFIG_NSH_ROMFSETC=y +# CONFIG_NSH_ROMFSRC is not set +CONFIG_NSH_ROMFSMOUNTPT="/etc" +CONFIG_NSH_INITSCRIPT="init.d/rcS" +CONFIG_NSH_ROMFSDEVNO=0 +CONFIG_NSH_ROMFSSECTSIZE=128 +CONFIG_NSH_ARCHROMFS=y +CONFIG_NSH_FATDEVNO=1 +CONFIG_NSH_FATSECTSIZE=512 +CONFIG_NSH_FATNSECTORS=1024 +CONFIG_NSH_FATMOUNTPT="/tmp" +CONFIG_NSH_CONSOLE=y + +# +# USB Trace Support +# +# CONFIG_NSH_CONDEV is not set +CONFIG_NSH_ARCHINIT=y + +# +# NxWidgets/NxWM +# + +# +# System NSH Add-Ons +# + +# +# Custom Free Memory Command +# +# CONFIG_SYSTEM_FREE is not set + +# +# I2C tool +# +CONFIG_SYSTEM_I2CTOOL=y +CONFIG_I2CTOOL_MINBUS=0 +CONFIG_I2CTOOL_MAXBUS=3 +CONFIG_I2CTOOL_MINADDR=0x03 +CONFIG_I2CTOOL_MAXADDR=0x77 +CONFIG_I2CTOOL_MAXREGADDR=0xff +CONFIG_I2CTOOL_DEFFREQ=4000000 + +# +# FLASH Program Installation +# +# CONFIG_SYSTEM_INSTALL is not set + +# +# FLASH Erase-all Command +# +# CONFIG_SYSTEM_FLASH_ERASEALL is not set + +# +# readline() +# +CONFIG_SYSTEM_READLINE=y +CONFIG_READLINE_ECHO=y + +# +# Power Off +# +# CONFIG_SYSTEM_POWEROFF is not set + +# +# RAMTRON +# +# CONFIG_SYSTEM_RAMTRON is not set + +# +# SD Card +# +# CONFIG_SYSTEM_SDCARD is not set + +# +# Sysinfo +# +CONFIG_SYSTEM_SYSINFO=y + +# +# USB Monitor +# diff --git a/nuttx-configs/aerocore/nsh/setenv.sh b/nuttx-configs/aerocore/nsh/setenv.sh new file mode 100755 index 000000000..2327f38a1 --- /dev/null +++ b/nuttx-configs/aerocore/nsh/setenv.sh @@ -0,0 +1,67 @@ +#!/bin/bash +# configs/aerocore/nsh/setenv.sh +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX 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. +# + +if [ "$_" = "$0" ] ; then + echo "You must source this script, not run it!" 1>&2 + exit 1 +fi + +WD=`pwd` +if [ ! -x "setenv.sh" ]; then + echo "This script must be executed from the top-level NuttX build directory" + exit 1 +fi + +if [ -z "${PATH_ORIG}" ]; then + export PATH_ORIG="${PATH}" +fi + +# This the Cygwin path to the location where I installed the RIDE +# toolchain under windows. You will also have to edit this if you install +# the RIDE toolchain in any other location +#export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/Raisonance/Ride/arm-gcc/bin" + +# This the Cygwin path to the location where I installed the CodeSourcery +# toolchain under windows. You will also have to edit this if you install +# the CodeSourcery toolchain in any other location +export TOOLCHAIN_BIN="/cygdrive/c/Program Files (x86)/CodeSourcery/Sourcery G++ Lite/bin" + +# This the Cygwin path to the location where I build the buildroot +# toolchain. +#export TOOLCHAIN_BIN="${WD}/../misc/buildroot/build_arm_nofpu/staging_dir/bin" + +# Add the path to the toolchain to the PATH varialble +export PATH="${TOOLCHAIN_BIN}:/sbin:/usr/sbin:${PATH_ORIG}" + +echo "PATH : ${PATH}" diff --git a/nuttx-configs/aerocore/scripts/ld.script b/nuttx-configs/aerocore/scripts/ld.script new file mode 100644 index 000000000..968d3127f --- /dev/null +++ b/nuttx-configs/aerocore/scripts/ld.script @@ -0,0 +1,150 @@ +/**************************************************************************** + * configs/aerocore/common/ld.script + * + * Copyright (C) 2011 Gregory Nutt. All rights reserved. + * Author: Gregory Nutt + * + * 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 NuttX 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. + * + ****************************************************************************/ + +/* The STM32F427 has 2048Kb of FLASH beginning at address 0x0800:0000 and + * 256Kb of SRAM. SRAM is split up into three blocks: + * + * 1) 112Kb of SRAM beginning at address 0x2000:0000 + * 2) 16Kb of SRAM beginning at address 0x2001:c000 + * 3) 64Kb of SRAM beginning at address 0x2002:0000 + * 4) 64Kb of TCM SRAM beginning at address 0x1000:0000 + * + * When booting from FLASH, FLASH memory is aliased to address 0x0000:0000 + * where the code expects to begin execution by jumping to the entry point in + * the 0x0800:0000 address range. + * + * The first 0x4000 of flash is reserved for the bootloader. + */ + +MEMORY +{ + flash (rx) : ORIGIN = 0x08004000, LENGTH = 2032K + sram (rwx) : ORIGIN = 0x20000000, LENGTH = 192K + ccsram (rwx) : ORIGIN = 0x10000000, LENGTH = 64K +} + +OUTPUT_ARCH(arm) + +ENTRY(__start) /* treat __start as the anchor for dead code stripping */ +EXTERN(_vectors) /* force the vectors to be included in the output */ + +/* + * Ensure that abort() is present in the final object. The exception handling + * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). + */ +EXTERN(abort) + +SECTIONS +{ + .text : { + _stext = ABSOLUTE(.); + *(.vectors) + *(.text .text.*) + *(.fixup) + *(.gnu.warning) + *(.rodata .rodata.*) + *(.gnu.linkonce.t.*) + *(.got) + *(.gcc_except_table) + *(.gnu.linkonce.r.*) + _etext = ABSOLUTE(.); + + /* + * This is a hack to make the newlib libm __errno() call + * use the NuttX get_errno_ptr() function. + */ + __errno = get_errno_ptr; + } > flash + + /* + * Init functions (static constructors and the like) + */ + .init_section : { + _sinit = ABSOLUTE(.); + KEEP(*(.init_array .init_array.*)) + _einit = ABSOLUTE(.); + } > flash + + /* + * Construction data for parameters. + */ + __param ALIGN(4): { + __param_start = ABSOLUTE(.); + KEEP(*(__param*)) + __param_end = ABSOLUTE(.); + } > flash + + .ARM.extab : { + *(.ARM.extab*) + } > flash + + __exidx_start = ABSOLUTE(.); + .ARM.exidx : { + *(.ARM.exidx*) + } > flash + __exidx_end = ABSOLUTE(.); + + _eronly = ABSOLUTE(.); + + .data : { + _sdata = ABSOLUTE(.); + *(.data .data.*) + *(.gnu.linkonce.d.*) + CONSTRUCTORS + _edata = ABSOLUTE(.); + } > sram AT > flash + + .bss : { + _sbss = ABSOLUTE(.); + *(.bss .bss.*) + *(.gnu.linkonce.b.*) + *(COMMON) + _ebss = ABSOLUTE(.); + } > sram + + /* Stabs debugging sections. */ + .stab 0 : { *(.stab) } + .stabstr 0 : { *(.stabstr) } + .stab.excl 0 : { *(.stab.excl) } + .stab.exclstr 0 : { *(.stab.exclstr) } + .stab.index 0 : { *(.stab.index) } + .stab.indexstr 0 : { *(.stab.indexstr) } + .comment 0 : { *(.comment) } + .debug_abbrev 0 : { *(.debug_abbrev) } + .debug_info 0 : { *(.debug_info) } + .debug_line 0 : { *(.debug_line) } + .debug_pubnames 0 : { *(.debug_pubnames) } + .debug_aranges 0 : { *(.debug_aranges) } +} diff --git a/nuttx-configs/aerocore/src/Makefile b/nuttx-configs/aerocore/src/Makefile new file mode 100644 index 000000000..859ba4ab2 --- /dev/null +++ b/nuttx-configs/aerocore/src/Makefile @@ -0,0 +1,84 @@ +############################################################################ +# configs/aerocore/src/Makefile +# +# Copyright (C) 2011 Gregory Nutt. All rights reserved. +# Author: Gregory Nutt +# +# 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 NuttX nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +-include $(TOPDIR)/Make.defs + +CFLAGS += -I$(TOPDIR)/sched + +ASRCS = +AOBJS = $(ASRCS:.S=$(OBJEXT)) + +CSRCS = empty.c +COBJS = $(CSRCS:.c=$(OBJEXT)) + +SRCS = $(ASRCS) $(CSRCS) +OBJS = $(AOBJS) $(COBJS) + +ARCH_SRCDIR = $(TOPDIR)/arch/$(CONFIG_ARCH)/src +ifeq ($(WINTOOL),y) + CFLAGS += -I "${shell cygpath -w $(ARCH_SRCDIR)/chip}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/common}" \ + -I "${shell cygpath -w $(ARCH_SRCDIR)/armv7-m}" +else + CFLAGS += -I$(ARCH_SRCDIR)/chip -I$(ARCH_SRCDIR)/common -I$(ARCH_SRCDIR)/armv7-m +endif + +all: libboard$(LIBEXT) + +$(AOBJS): %$(OBJEXT): %.S + $(call ASSEMBLE, $<, $@) + +$(COBJS) $(LINKOBJS): %$(OBJEXT): %.c + $(call COMPILE, $<, $@) + +libboard$(LIBEXT): $(OBJS) + $(call ARCHIVE, $@, $(OBJS)) + +.depend: Makefile $(SRCS) + $(Q) $(MKDEP) $(CC) -- $(CFLAGS) -- $(SRCS) >Make.dep + $(Q) touch $@ + +depend: .depend + +clean: + $(call DELFILE, libboard$(LIBEXT)) + $(call CLEAN) + +distclean: clean + $(call DELFILE, Make.dep) + $(call DELFILE, .depend) + +-include Make.dep + diff --git a/nuttx-configs/aerocore/src/empty.c b/nuttx-configs/aerocore/src/empty.c new file mode 100644 index 000000000..ace900866 --- /dev/null +++ b/nuttx-configs/aerocore/src/empty.c @@ -0,0 +1,4 @@ +/* + * There are no source files here, but libboard.a can't be empty, so + * we have this empty source file to keep it company. + */ diff --git a/src/drivers/boards/aerocore/aerocore_init.c b/src/drivers/boards/aerocore/aerocore_init.c new file mode 100644 index 000000000..4e3ba2d7e --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_init.c @@ -0,0 +1,282 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 aerocore_init.c + * + * AeroCore-specific early startup code. This file implements the + * nsh_archinitialize() function that is called early by nsh during startup. + * + * Code here is run before the rcS script is invoked; it should start required + * subsystems and perform board-specific initialisation. + */ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include "board_config.h" +#include + +#include + +#include +#include + +#include +#include + +/**************************************************************************** + * Pre-Processor Definitions + ****************************************************************************/ + +/* Configuration ************************************************************/ + +/* Debug ********************************************************************/ + +#ifdef CONFIG_CPP_HAVE_VARARGS +# ifdef CONFIG_DEBUG +# define message(...) lowsyslog(__VA_ARGS__) +# else +# define message(...) printf(__VA_ARGS__) +# endif +#else +# ifdef CONFIG_DEBUG +# define message lowsyslog +# else +# define message printf +# endif +#endif + +/**************************************************************************** + * Protected Functions + ****************************************************************************/ + +#if defined(CONFIG_FAT_DMAMEMORY) +# if !defined(CONFIG_GRAN) || !defined(CONFIG_FAT_DMAMEMORY) +# error microSD DMA support requires CONFIG_GRAN +# endif + +static GRAN_HANDLE dma_allocator; + +/* + * The DMA heap size constrains the total number of things that can be + * ready to do DMA at a time. + * + * For example, FAT DMA depends on one sector-sized buffer per filesystem plus + * one sector-sized buffer per file. + * + * We use a fundamental alignment / granule size of 64B; this is sufficient + * to guarantee alignment for the largest STM32 DMA burst (16 beats x 32bits). + */ +static uint8_t g_dma_heap[8192] __attribute__((aligned(64))); +static perf_counter_t g_dma_perf; + +static void +dma_alloc_init(void) +{ + dma_allocator = gran_initialize(g_dma_heap, + sizeof(g_dma_heap), + 7, /* 128B granule - must be > alignment (XXX bug?) */ + 6); /* 64B alignment */ + if (dma_allocator == NULL) { + message("[boot] DMA allocator setup FAILED"); + } else { + g_dma_perf = perf_alloc(PC_COUNT, "DMA allocations"); + } +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/* + * DMA-aware allocator stubs for the FAT filesystem. + */ + +__EXPORT void *fat_dma_alloc(size_t size); +__EXPORT void fat_dma_free(FAR void *memory, size_t size); + +void * +fat_dma_alloc(size_t size) +{ + perf_count(g_dma_perf); + return gran_alloc(dma_allocator, size); +} + +void +fat_dma_free(FAR void *memory, size_t size) +{ + gran_free(dma_allocator, memory, size); +} + +#else + +# define dma_alloc_init() + +#endif + +/************************************************************************************ + * Name: stm32_boardinitialize + * + * Description: + * All STM32 architectures must provide the following entry point. This entry point + * is called early in the intitialization -- after all memory has been configured + * and mapped but before any devices have been initialized. + * + ************************************************************************************/ + +__EXPORT void +stm32_boardinitialize(void) +{ + /* configure SPI interfaces */ + stm32_spiinitialize(); + + /* configure LEDs */ + up_ledinit(); +} + +/**************************************************************************** + * Name: nsh_archinitialize + * + * Description: + * Perform architecture specific initialization + * + ****************************************************************************/ + +static struct spi_dev_s *spi3; +static struct spi_dev_s *spi4; + +#include + +#ifdef __cplusplus +__EXPORT int matherr(struct __exception *e) +{ + return 1; +} +#else +__EXPORT int matherr(struct exception *e) +{ + return 1; +} +#endif + +__EXPORT int nsh_archinitialize(void) +{ + + /* configure ADC pins */ + stm32_configgpio(GPIO_ADC1_IN10); /* used by VBUS valid */ + stm32_configgpio(GPIO_ADC1_IN11); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN12); /* J1 breakout */ + stm32_configgpio(GPIO_ADC1_IN13); /* J1 breakout */ + + /* configure the high-resolution time/callout interface */ + hrt_init(); + + /* configure the DMA allocator */ + dma_alloc_init(); + + /* configure CPU load estimation */ +#ifdef CONFIG_SCHED_INSTRUMENTATION + cpuload_initialize_once(); +#endif + + /* set up the serial DMA polling */ + static struct hrt_call serial_dma_call; + struct timespec ts; + + /* + * Poll at 1ms intervals for received bytes that have not triggered + * a DMA event. + */ + ts.tv_sec = 0; + ts.tv_nsec = 1000000; + + hrt_call_every(&serial_dma_call, + ts_to_abstime(&ts), + ts_to_abstime(&ts), + (hrt_callout)stm32_serial_dma_poll, + NULL); + + /* initial LED state */ + drv_led_start(); + led_off(LED_AMBER); + + /* Configure Sensors on SPI bus #3 */ + spi3 = up_spiinitialize(3); + if (!spi3) { + message("[boot] FAILED to initialize SPI port 3\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: 1MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi3, 10000000); + SPI_SETBITS(spi3, 8); + SPI_SETMODE(spi3, SPIDEV_MODE3); + SPI_SELECT(spi3, PX4_SPIDEV_GYRO, false); + SPI_SELECT(spi3, PX4_SPIDEV_ACCEL_MAG, false); + SPI_SELECT(spi3, PX4_SPIDEV_BARO, false); + up_udelay(20); + message("[boot] Initialized SPI port 3 (SENSORS)\n"); + + /* Configure FRAM on SPI bus #4 */ + spi4 = up_spiinitialize(4); + if (!spi4) { + message("[boot] FAILED to initialize SPI port 4\n"); + up_ledon(LED_AMBER); + return -ENODEV; + } + /* Default: ~10MHz, 8 bits, Mode 3 */ + SPI_SETFREQUENCY(spi4, 10 * 1000 * 1000); + SPI_SETBITS(spi4, 8); + SPI_SETMODE(spi4, SPIDEV_MODE0); + SPI_SELECT(spi4, SPIDEV_FLASH, false); + message("[boot] Initialized SPI port 4 (FRAM)\n"); + + return OK; +} diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c new file mode 100644 index 000000000..e40d1730c --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -0,0 +1,121 @@ +/**************************************************************************** + * + * 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 aerocore_led.c + * + * AeroCore LED backend. + */ + +#include + +#include + +#include "stm32.h" +#include "board_config.h" + +#include + +/* + * Ideally we'd be able to get these from up_internal.h, + * but since we want to be able to disable the NuttX use + * of leds for system indication at will and there is no + * separate switch, we need to build independent of the + * CONFIG_ARCH_LEDS configuration switch. + */ +__BEGIN_DECLS +extern void led_init(); +extern void led_on(int led); +extern void led_off(int led); +extern void led_toggle(int led); +__END_DECLS + +__EXPORT void led_init() +{ + stm32_configgpio(GPIO_LED0); + stm32_configgpio(GPIO_LED1); +} + +__EXPORT void led_on(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_off(int led) +{ + switch (led) { + case 0: + stm32_gpiowrite(GPIO_LED0, false); + break; + + case 1: + stm32_gpiowrite(GPIO_LED1, false); + break; + + default: + warnx("LED ID not recognized\n"); + } +} + +__EXPORT void led_toggle(int led) +{ + switch (led) { + case 0: + if (stm32_gpioread(GPIO_LED0)) + stm32_gpiowrite(GPIO_LED0, false); + else + stm32_gpiowrite(GPIO_LED0, true); + break; + + case 1: + if (stm32_gpioread(GPIO_LED1)) + stm32_gpiowrite(GPIO_LED1, false); + else + stm32_gpiowrite(GPIO_LED1, true); + break; + + default: + warnx("LED ID not recognized\n"); + } +} diff --git a/src/drivers/boards/aerocore/aerocore_pwm_servo.c b/src/drivers/boards/aerocore/aerocore_pwm_servo.c new file mode 100644 index 000000000..251eaff7b --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_pwm_servo.c @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 aerocore_pwm_servo.c + * + * Configuration data for the stm32 pwm_servo driver. + * + * Note that these arrays must always be fully-sized. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "board_config.h" + +__EXPORT const struct pwm_servo_timer pwm_timers[PWM_SERVO_MAX_TIMERS] = { + { + .base = STM32_TIM1_BASE, + .clock_register = STM32_RCC_APB2ENR, + .clock_bit = RCC_APB2ENR_TIM1EN, + .clock_freq = STM32_APB2_TIM1_CLKIN + }, + { + .base = STM32_TIM3_BASE, + .clock_register = STM32_RCC_APB1ENR, + .clock_bit = RCC_APB1ENR_TIM3EN, + .clock_freq = STM32_APB1_TIM3_CLKIN + } +}; + +__EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { + { + .gpio = GPIO_TIM1_CH1OUT, + .timer_index = 0, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH2OUT, + .timer_index = 0, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH3OUT, + .timer_index = 0, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM1_CH4OUT, + .timer_index = 0, + .timer_channel = 4, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH1OUT, + .timer_index = 1, + .timer_channel = 1, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH2OUT, + .timer_index = 1, + .timer_channel = 2, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH3OUT, + .timer_index = 1, + .timer_channel = 3, + .default_value = 1500, + }, + { + .gpio = GPIO_TIM3_CH4OUT, + .timer_index = 1, + .timer_channel = 4, + .default_value = 1500, + } +}; diff --git a/src/drivers/boards/aerocore/aerocore_spi.c b/src/drivers/boards/aerocore/aerocore_spi.c new file mode 100644 index 000000000..e329bd9d1 --- /dev/null +++ b/src/drivers/boards/aerocore/aerocore_spi.c @@ -0,0 +1,183 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 aerocore_spi.c + * + * Board-specific SPI functions. + */ + +/************************************************************************************ + * Included Files + ************************************************************************************/ + +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "board_config.h" + +/************************************************************************************ + * Public Functions + ************************************************************************************/ + +/************************************************************************************ + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ************************************************************************************/ + +__EXPORT void weak_function stm32_spiinitialize(void) +{ +#ifdef CONFIG_STM32_SPI1 + stm32_configgpio(GPIO_SPI1_NSS); + stm32_gpiowrite(GPIO_SPI1_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI2 + stm32_configgpio(GPIO_SPI2_NSS); + stm32_gpiowrite(GPIO_SPI2_NSS, 1); +#endif + +#ifdef CONFIG_STM32_SPI3 + stm32_configgpio(GPIO_SPI_CS_GYRO); + stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); + stm32_configgpio(GPIO_SPI_CS_BARO); + + /* De-activate all peripherals, + * required for some peripheral + * state machines + */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + + stm32_configgpio(GPIO_EXTI_GYRO_DRDY); + stm32_configgpio(GPIO_EXTI_MAG_DRDY); + stm32_configgpio(GPIO_EXTI_ACCEL_DRDY); +#endif + +#ifdef CONFIG_STM32_SPI4 + stm32_configgpio(GPIO_SPI4_NSS); + stm32_gpiowrite(GPIO_SPI4_NSS, 1); +#endif +} + +#ifdef CONFIG_STM32_SPI1 +__EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI1_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI2 +__EXPORT void stm32_spi2select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there is only one device broken-out so select it */ + stm32_gpiowrite(GPIO_SPI2_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + +#ifdef CONFIG_STM32_SPI3 +__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* SPI select is active low, so write !selected to select the device */ + + switch (devid) { + case PX4_SPIDEV_GYRO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_ACCEL_MAG: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + break; + + case PX4_SPIDEV_BARO: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + break; + + default: + break; + } +} + +__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + return SPI_STATUS_PRESENT; +} +#endif + + +#ifdef CONFIG_STM32_SPI4 +__EXPORT void stm32_spi4select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected) +{ + /* there can only be one device on this bus, so always select it */ + stm32_gpiowrite(GPIO_SPI4_NSS, !selected); +} + +__EXPORT uint8_t stm32_spi4status(FAR struct spi_dev_s *dev, enum spi_dev_e devid) +{ + /* FRAM is always present */ + return SPI_STATUS_PRESENT; +} +#endif diff --git a/src/drivers/boards/aerocore/board_config.h b/src/drivers/boards/aerocore/board_config.h new file mode 100644 index 000000000..a3257f339 --- /dev/null +++ b/src/drivers/boards/aerocore/board_config.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2013, 2014 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 board_config.h + * + * AeroCore internal definitions + */ + +#pragma once + +/**************************************************************************************************** + * Included Files + ****************************************************************************************************/ + +#include +#include +#include + +__BEGIN_DECLS + +/* these headers are not C++ safe */ +#include +#include + +#define UDID_START 0x1FFF7A10 + +/**************************************************************************************************** + * Definitions + ****************************************************************************************************/ + +/* LEDs */ +#define GPIO_LED0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN9) +#define GPIO_LED1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN10) + +/* Gyro */ +#define GPIO_EXTI_GYRO_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN0) + +/* Accel & Mag */ +#define GPIO_EXTI_MAG_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN1) +#define GPIO_EXTI_ACCEL_DRDY (GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTD|GPIO_PIN2) + +/* GPS */ +#define GPIO_GPS_NRESET (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN5) +#define GPIO_GPS_TIMEPULSE (GPIO_INPUT|GPIO_FLOAT|GPIO_PORTC|GPIO_PIN4) + +/* SPI3--Sensors */ +#define PX4_SPI_BUS_SENSORS 3 +#define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN2) +#define GPIO_SPI_CS_GYRO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN3) +#define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) + +/* Nominal chip selects for devices on SPI bus #3 */ +#define PX4_SPIDEV_ACCEL_MAG 0 +#define PX4_SPIDEV_GYRO 1 +#define PX4_SPIDEV_BARO 2 + +/* User GPIOs broken out on J11 */ +#define GPIO_GPIO0_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN1) +//#define GPIO_GPIO2_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_INPUT (GPIO_INPUT|GPIO_PULLUP|GPIO_PORTB|GPIO_PIN8) + +#define GPIO_GPIO0_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN0) +#define GPIO_GPIO1_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN1) +//#define GPIO_GPIO2_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_GPIO3_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN1) +#define GPIO_GPIO4_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN2) +#define GPIO_GPIO5_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN3) +#define GPIO_GPIO6_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN12) +#define GPIO_GPIO7_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN13) +#define GPIO_GPIO8_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN14) +#define GPIO_GPIO9_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTD|GPIO_PIN15) +#define GPIO_GPIO10_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN5) +#define GPIO_GPIO11_OUTPUT (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTB|GPIO_PIN8) + +/* PWM + * + * Eight PWM outputs are configured. + * + * Pins: + * + * CH1 : PA8 : TIM1_CH1 + * CH2 : PA9 : TIM1_CH2 + * CH3 : PA10 : TIM1_CH3 + * CH4 : PA11 : TIM1_CH4 + * CH5 : PC6 : TIM3_CH1 + * CH6 : PC7 : TIM3_CH2 + * CH7 : PC8 : TIM3_CH3 + * CH8 : PC9 : TIM3_CH4 + */ +#define GPIO_TIM1_CH1OUT GPIO_TIM1_CH1OUT_1 +#define GPIO_TIM1_CH2OUT GPIO_TIM1_CH2OUT_1 +#define GPIO_TIM1_CH3OUT GPIO_TIM1_CH3OUT_1 +#define GPIO_TIM1_CH4OUT GPIO_TIM1_CH4OUT_1 +#define GPIO_TIM3_CH1OUT GPIO_TIM3_CH1OUT_3 +#define GPIO_TIM3_CH2OUT GPIO_TIM3_CH2OUT_3 +#define GPIO_TIM3_CH3OUT GPIO_TIM3_CH3OUT_2 +#define GPIO_TIM3_CH4OUT GPIO_TIM3_CH4OUT_2 + +/* High-resolution timer */ +#define HRT_TIMER 8 /* use timer 8 for the HRT */ +#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */ + +/* Tone Alarm (no onboard speaker )*/ +#define TONE_ALARM_TIMER 2 /* timer 2 */ +#define TONE_ALARM_CHANNEL 1 /* channel 1 */ +#define GPIO_TONE_ALARM_IDLE (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN0) +#define GPIO_TONE_ALARM (GPIO_ALT|GPIO_AF1|GPIO_SPEED_2MHz|GPIO_PUSHPULL|GPIO_PORTA|GPIO_PIN0) + + +/**************************************************************************************************** + * Public Types + ****************************************************************************************************/ + +/**************************************************************************************************** + * Public data + ****************************************************************************************************/ + +#ifndef __ASSEMBLY__ + +/**************************************************************************************************** + * Public Functions + ****************************************************************************************************/ + +/**************************************************************************************************** + * Name: stm32_spiinitialize + * + * Description: + * Called to configure SPI chip select GPIO pins for the PX4FMU board. + * + ****************************************************************************************************/ + +extern void stm32_spiinitialize(void); + +#endif /* __ASSEMBLY__ */ + +__END_DECLS diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk new file mode 100644 index 000000000..b53fe0a29 --- /dev/null +++ b/src/drivers/boards/aerocore/module.mk @@ -0,0 +1,8 @@ +# +# Board-specific startup code for the AeroCore +# + +SRCS = aerocore_init.c \ + aerocore_pwm_servo.c \ + aerocore_spi.c \ + aerocore_led.c diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h index 02c26b5c0..58273f2d2 100644 --- a/src/drivers/boards/px4fmu-v1/board_config.h +++ b/src/drivers/boards/px4fmu-v1/board_config.h @@ -85,6 +85,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTB|GPIO_PIN0) #define GPIO_SPI_CS_SDCARD (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN4) +#define PX4_SPI_BUS_SENSORS 1 + /* * Use these in place of the spi_dev_e enumeration to * select a specific SPI device on SPI1 diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index 7cfca7656..c2de1bfba 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -107,6 +107,8 @@ __BEGIN_DECLS #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) +#define PX4_SPI_BUS_SENSORS 1 + /* Use these in place of the spi_dev_e enumeration to select a specific SPI device on SPI1 */ #define PX4_SPIDEV_GYRO 1 #define PX4_SPIDEV_ACCEL_MAG 2 diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index f60964c2b..5acd0d343 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -94,6 +94,14 @@ #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +/* + * AeroCore GPIO numbers and configuration. + * + */ +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +#endif + #ifdef CONFIG_ARCH_BOARD_PX4IO_V1 /* no GPIO driver on the PX4IOv1 board */ #endif @@ -146,4 +154,4 @@ #define GPIO_SENSOR_RAIL_RESET GPIOC(13) -#endif /* _DRV_GPIO_H */ \ No newline at end of file +#endif /* _DRV_GPIO_H */ diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 06e3535b3..96669a021 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -46,7 +46,11 @@ #include "drv_sensor.h" #include "drv_orb_dev.h" +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define GPS_DEFAULT_UART_PORT "/dev/ttyS0" +#else #define GPS_DEFAULT_UART_PORT "/dev/ttyS3" +#endif #define GPS_DEVICE_PATH "/dev/gps" diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f..337705570 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -63,6 +63,8 @@ #include #include +#include + #include "ubx.h" #include "mtk.h" @@ -421,7 +423,14 @@ GPS::task_main() void GPS::cmd_reset() { - //XXX add reset? +#ifdef GPIO_GPS_NRESET + warnx("Toggling GPS reset pin"); + stm32_configgpio(GPIO_GPS_NRESET); + stm32_gpiowrite(GPIO_GPS_NRESET, 0); + usleep(100); + stm32_gpiowrite(GPIO_GPS_NRESET, 1); + warnx("Toggled GPS reset pin"); +#endif } void diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4dee7649b..4ca8b5e42 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1793,7 +1793,7 @@ start() errx(0, "already started"); /* create the driver */ - g_dev = new LSM303D(1 /* SPI dev 1 */, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); + g_dev = new LSM303D(PX4_SPI_BUS_SENSORS, LSM303D_DEVICE_PATH_ACCEL, (spi_dev_e)PX4_SPIDEV_ACCEL_MAG); if (g_dev == nullptr) { warnx("failed instantiating LSM303D obj"); diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 26216e840..8759d16a1 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -117,7 +117,7 @@ private: device::Device * MS5611_spi_interface(ms5611::prom_u &prom_buf) { - return new MS5611_SPI(1 /* XXX MAGIC NUMBER */, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); + return new MS5611_SPI(PX4_SPI_BUS_SENSORS, (spi_dev_e)PX4_SPIDEV_BARO, prom_buf); } MS5611_SPI::MS5611_SPI(int bus, spi_dev_e device, ms5611::prom_u &prom_buf) : diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 4d72ead9b..1b6b136eb 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -92,6 +92,7 @@ public: MODE_2PWM, MODE_4PWM, MODE_6PWM, + MODE_8PWM, }; PX4FMU(); virtual ~PX4FMU(); @@ -113,6 +114,9 @@ private: #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) static const unsigned _max_actuators = 6; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + static const unsigned _max_actuators = 8; +#endif Mode _mode; unsigned _pwm_default_rate; @@ -203,6 +207,21 @@ const PX4FMU::GPIOConfig PX4FMU::_gpio_tab[] = { {GPIO_VDD_5V_HIPOWER_OC, 0, 0}, {GPIO_VDD_5V_PERIPH_OC, 0, 0}, #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + /* AeroCore breaks out User GPIOs on J11 */ + {GPIO_GPIO0_INPUT, GPIO_GPIO0_OUTPUT, 0}, + {GPIO_GPIO1_INPUT, GPIO_GPIO1_OUTPUT, 0}, +// {GPIO_GPIO2_INPUT, GPIO_GPIO2_OUTPUT, 0}, + {GPIO_GPIO3_INPUT, GPIO_GPIO3_OUTPUT, 0}, + {GPIO_GPIO4_INPUT, GPIO_GPIO4_OUTPUT, 0}, + {GPIO_GPIO5_INPUT, GPIO_GPIO5_OUTPUT, 0}, + {GPIO_GPIO6_INPUT, GPIO_GPIO6_OUTPUT, 0}, + {GPIO_GPIO7_INPUT, GPIO_GPIO7_OUTPUT, 0}, + {GPIO_GPIO8_INPUT, GPIO_GPIO8_OUTPUT, 0}, + {GPIO_GPIO9_INPUT, GPIO_GPIO9_OUTPUT, 0}, + {GPIO_GPIO10_INPUT, GPIO_GPIO10_OUTPUT, 0}, + {GPIO_GPIO11_INPUT, GPIO_GPIO11_OUTPUT, 0}, +#endif }; const unsigned PX4FMU::_ngpio = sizeof(PX4FMU::_gpio_tab) / sizeof(PX4FMU::_gpio_tab[0]); @@ -382,6 +401,20 @@ PX4FMU::set_mode(Mode mode) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: // AeroCore PWMs as 8 PWM outs + debug("MODE_8PWM"); + /* default output rates */ + _pwm_default_rate = 50; + _pwm_alt_rate = 50; + _pwm_alt_rate_channels = 0; + + /* XXX magic numbers */ + up_pwm_servo_init(0xff); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + break; +#endif + case MODE_NONE: debug("MODE_NONE"); @@ -602,6 +635,9 @@ PX4FMU::task_main() num_outputs = 6; break; + case MODE_8PWM: + num_outputs = 8; + break; default: num_outputs = 0; break; @@ -757,6 +793,9 @@ PX4FMU::ioctl(file *filp, int cmd, unsigned long arg) case MODE_2PWM: case MODE_4PWM: case MODE_6PWM: +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: +#endif ret = pwm_ioctl(filp, cmd, arg); break; @@ -986,6 +1025,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_SET(7): + case PWM_SERVO_SET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_SET(5): case PWM_SERVO_SET(4): if (_mode < MODE_6PWM) { @@ -1013,6 +1061,15 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): + if (_mode < MODE_8PWM) { + ret = -EINVAL; + break; + } +#endif + case PWM_SERVO_GET(5): case PWM_SERVO_GET(4): if (_mode < MODE_6PWM) { @@ -1040,12 +1097,22 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) case PWM_SERVO_GET_RATEGROUP(3): case PWM_SERVO_GET_RATEGROUP(4): case PWM_SERVO_GET_RATEGROUP(5): +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case PWM_SERVO_GET_RATEGROUP(6): + case PWM_SERVO_GET_RATEGROUP(7): +#endif *(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0)); break; case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: switch (_mode) { +#ifdef CONFIG_ARCH_BOARD_AEROCORE + case MODE_8PWM: + *(unsigned *)arg = 8; + break; +#endif + case MODE_6PWM: *(unsigned *)arg = 6; break; @@ -1091,6 +1158,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) set_mode(MODE_6PWM); break; #endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + case 8: + set_mode(MODE_8PWM); + break; +#endif default: ret = -EINVAL; @@ -1181,10 +1253,17 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) unsigned count = len / 2; uint16_t values[6]; +#ifdef CONFIG_ARCH_BOARD_AEROCORE + if (count > 8) { + // we have at most 8 outputs + count = 8; + } +#else if (count > 6) { // we have at most 6 outputs count = 6; } +#endif // allow for misaligned values memcpy(values, buffer, count * 2); @@ -1458,6 +1537,9 @@ fmu_new_mode(PortMode new_mode) #endif #if defined(CONFIG_ARCH_BOARD_PX4FMU_V2) servo_mode = PX4FMU::MODE_6PWM; +#endif +#if defined(CONFIG_ARCH_BOARD_AEROCORE) + servo_mode = PX4FMU::MODE_8PWM; #endif break; @@ -1776,7 +1858,7 @@ fmu_main(int argc, char *argv[]) fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb); #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n"); -#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) +#elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2) || defined(CONFIG_ARCH_BOARD_AEROCORE) fprintf(stderr, " mode_gpio, mode_pwm, test, sensor_reset [milliseconds]\n"); #endif exit(1); diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 3a60d2cae..de13b8969 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -419,6 +419,10 @@ adc_main(int argc, char *argv[]) g_adc = new ADC((1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15)); #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE + /* XXX this hardcodes the default channel set for AeroCore - should be configurable */ + g_adc = new ADC((1 << 10) | (1 << 11) | (1 << 12) | (1 << 13)); +#endif if (g_adc == nullptr) errx(1, "couldn't allocate the ADC driver"); diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 5bb550279..281f918d7 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -141,7 +141,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM10EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1UP -# define HRT_TIMER_CLOCK STM32_APB1_TIM10_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM10_CLKIN # if CONFIG_STM32_TIM10 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=10 # endif @@ -150,7 +150,7 @@ # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR # define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM11EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM1TRGCOM -# define HRT_TIMER_CLOCK STM32_APB1_TIM11_CLKIN +# define HRT_TIMER_CLOCK STM32_APB2_TIM11_CLKIN # if CONFIG_STM32_TIM11 # error must not set CONFIG_STM32_TIM11=y and HRT_TIMER=11 # endif diff --git a/src/lib/version/version.h b/src/lib/version/version.h index af733aaf0..d8ccb6774 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -59,4 +59,8 @@ #define HW_ARCH "PX4FMU_V2" #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define HW_ARCH "AEROCORE" +#endif + #endif /* VERSION_H_ */ diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 86f77cdb8..940a04aa1 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -206,7 +206,7 @@ int led_init() return ERROR; } - /* the blue LED is only available on FMUv1 but not FMUv2 */ + /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ (void)ioctl(leds, LED_ON, LED_BLUE); /* we consider the amber led mandatory */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 07be3560a..c021e1fde 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -488,6 +488,15 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); +#elif CONFIG_ARCH_BOARD_AEROCORE +/** + * Scaling factor for battery voltage sensor on AeroCore. + * + * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063 + * + * @group Battery Calibration + */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f); #else /** * Scaling factor for battery voltage sensor on FMU v1. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 18bf97f8d..d31016111 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -126,6 +126,12 @@ #define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 #endif +#ifdef CONFIG_ARCH_BOARD_AEROCORE +#define ADC_BATTERY_VOLTAGE_CHANNEL 10 +#define ADC_BATTERY_CURRENT_CHANNEL -1 +#define ADC_AIRSPEED_VOLTAGE_CHANNEL -1 +#endif + #define BATT_V_LOWPASS 0.001f #define BATT_V_IGNORE_THRESHOLD 3.5f @@ -797,7 +803,7 @@ Sensors::accel_init() /* set the driver to poll at 1000Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 1000); -#elif CONFIG_ARCH_BOARD_PX4FMU_V2 +#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE /* set the accel internal sampling rate up to at leat 800Hz */ ioctl(fd, ACCELIOCSSAMPLERATE, 800); @@ -806,7 +812,7 @@ Sensors::accel_init() ioctl(fd, SENSORIOCSPOLLRATE, 800); #else -#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2 +#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE #endif diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 0a88d40f3..a57eaafe7 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -160,7 +160,11 @@ static void ramtron_attach(void) { /* find the right spi */ +#ifdef CONFIG_ARCH_BOARD_AEROCORE + struct spi_dev_s *spi = up_spiinitialize(4); +#else struct spi_dev_s *spi = up_spiinitialize(2); +#endif /* this resets the spi bus, set correct bus speed again */ SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); SPI_SETBITS(spi, 8); -- cgit v1.2.3 From 63905265ebccd3a2d7fd94d5cf4f2c02102481f3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 13 May 2014 16:05:47 +0200 Subject: Save code size on commander, reduce stack size of starting tool (NOT OF THE APP ITSELF!) --- src/modules/commander/module.mk | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 554dfcb08..27ca5c182 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -47,3 +47,7 @@ SRCS = commander.cpp \ baro_calibration.cpp \ rc_calibration.cpp \ airspeed_calibration.cpp + +MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 531ba79e55e9ccf9396ee30f067c933b4ec9c649 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 May 2014 19:34:37 +0200 Subject: Reduce commander stack size mildly --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17d3d3dcd..f92c7fc31 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2800, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); -- cgit v1.2.3 From 8dc0a21a7ee83dea5b3a3ac719c6a9e64aff78f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 07:14:58 +0200 Subject: mavlink, commander: Get back close to original stack sizes. Although tests came clean, we do not want to take any chances --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f92c7fc31..17d3d3dcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2800, + 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d222f9edc..611a8b804 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2193,7 +2193,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 1800, + 2000, (main_t)&Mavlink::start_helper, (const char **)argv); -- cgit v1.2.3 From 4bc06381a9b44e7da22f21c20082146ad5d29c1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 08:15:59 +0200 Subject: commander: Cleanup properly after out-of-mem error --- src/modules/commander/mag_calibration.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 9296db6ed..388820511 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -121,6 +121,20 @@ int do_mag_calibration(int mavlink_fd) if (x == NULL || y == NULL || z == NULL) { mavlink_log_critical(mavlink_fd, "ERROR: out of memory"); + + /* clean up */ + if (x != NULL) { + free(x); + } + + if (y != NULL) { + free(y); + } + + if (z != NULL) { + free(z); + } + res = ERROR; return res; } -- cgit v1.2.3 From 1e13b5a02c4aab5b32f43b6bf2b3588674ee84ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 May 2014 09:02:31 +0200 Subject: commander: Reduce calibration count, minimally reduce stack sizes after careful inspection --- src/modules/commander/commander.cpp | 4 ++-- src/modules/commander/mag_calibration.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 17d3d3dcd..13da27dcd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -249,7 +249,7 @@ int commander_main(int argc, char *argv[]) daemon_task = task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3000, + 2950, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -743,7 +743,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, 2992); + pthread_attr_setstacksize(&commander_low_prio_attr, 2900); struct sched_param param; (void)pthread_attr_getschedparam(&commander_low_prio_attr, ¶m); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 388820511..0ead22f77 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd) uint64_t calibration_interval = 45 * 1000 * 1000; /* maximum 500 values */ - const unsigned int calibration_maxcount = 500; + const unsigned int calibration_maxcount = 240; unsigned int calibration_counter; struct mag_scale mscale_null = { -- cgit v1.2.3 From 692e8f84a93a932986004d896554a70380ea11e9 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 May 2014 22:12:07 +0200 Subject: commander: don't require good EPH for local_position_valid, position_estimator_inav: estimate EPH/EPV and publish it in local position topic --- src/modules/commander/commander.cpp | 21 ++++++++++++++++++++- .../position_estimator_inav_main.c | 21 +++++++++++++++++---- src/modules/sdlog2/sdlog2.c | 10 ++++++++-- src/modules/sdlog2/sdlog2_messages.h | 7 ++++--- src/modules/uORB/topics/vehicle_local_position.h | 2 ++ 5 files changed, 51 insertions(+), 10 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd..e4ae357d7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1012,7 +1012,26 @@ 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 && eph_epv_good, &(status.condition_local_position_valid), &status_changed); + /* hysteresis for EPH */ + bool local_eph_good; + + if (status.condition_global_position_valid) { + if (local_position.eph > eph_epv_threshold * 2.0f) { + local_eph_good = false; + + } else { + local_eph_good = true; + } + + } else { + if (local_position.eph < eph_epv_threshold) { + local_eph_good = true; + + } else { + local_eph_good = false; + } + } + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(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); static bool published_condition_landed_fw = false; 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 d8d0ff37d..fdc3233e0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -199,6 +199,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float eph = 1.0; + float epv = 1.0; + float x_est_prev[3], y_est_prev[3], z_est_prev[3]; memset(x_est_prev, 0, sizeof(x_est_prev)); memset(y_est_prev, 0, sizeof(y_est_prev)); @@ -535,6 +538,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_valid = true; + eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm + } else { w_flow = 0.0f; flow_valid = false; @@ -673,6 +678,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } + eph = fminf(eph, gps.eph_m); + epv = fminf(epv, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); } @@ -712,6 +720,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) dt = fmaxf(fminf(0.02, dt), 0.002); // constrain dt from 2 to 20 ms t_prev = t; + /* increase EPH/EPV on each step */ + eph *= 1.0 + dt; + epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W; @@ -723,7 +735,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = (t < xy_src_time + xy_src_timeout); + bool can_estimate_xy = eph < max_eph_epv * 1.5; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -922,6 +934,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.landed = landed; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; + local_pos.eph = eph; + local_pos.epv = epv; if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; @@ -950,9 +964,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.yaw = local_pos.yaw; - // TODO implement dead-reckoning - global_pos.eph = gps.eph_m; - global_pos.epv = gps.epv_m; + global_pos.eph = eph; + global_pos.epv = epv; if (vehicle_global_position_pub < 0) { vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39f433eb5..70ce76806 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1119,10 +1119,16 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7; log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7; log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; - log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0); - log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0); + log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) | + (buf.local_pos.z_valid ? 2 : 0) | + (buf.local_pos.v_xy_valid ? 4 : 0) | + (buf.local_pos.v_z_valid ? 8 : 0) | + (buf.local_pos.xy_global ? 16 : 0) | + (buf.local_pos.z_global ? 32 : 0); log_msg.body.log_LPOS.landed = buf.local_pos.landed; log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0); + log_msg.body.log_LPOS.eph = buf.local_pos.eph; + log_msg.body.log_LPOS.epv = buf.local_pos.epv; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0c6188657..90025b9ff 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -109,10 +109,11 @@ struct log_LPOS_s { int32_t ref_lat; int32_t ref_lon; float ref_alt; - uint8_t xy_flags; - uint8_t z_flags; + uint8_t pos_flags; uint8_t landed; uint8_t ground_dist_flags; + float eph; + float epv; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -360,7 +361,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, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), - LOG_FORMAT(LPOS, "ffffffffLLfBBBB", "X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"), + LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), 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 a15303ea4..5d39c897d 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -83,6 +83,8 @@ struct vehicle_local_position_s { float dist_bottom_rate; /**< Distance to bottom surface (ground) change rate */ uint64_t surface_bottom_timestamp; /**< Time when new bottom surface found */ bool dist_bottom_valid; /**< true if distance to bottom surface is valid */ + float eph; + float epv; }; /** -- cgit v1.2.3 From 32ae2dd1d094f4554f5acdad8fb76ed9eb3ba1f0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 May 2014 11:00:14 +0200 Subject: commander: missed 'break' in 'switch' added --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 13da27dcd..504696ff9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1710,6 +1710,7 @@ set_control_mode() case MAIN_STATE_AUTO: navigator_enabled = true; + break; default: break; -- cgit v1.2.3 From 559bfbb11cdac499cda8070c32e2b7e79b3b883e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 20 May 2014 16:42:11 +0200 Subject: commander: allow disarm in ACRO mode --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8336bcf32..45ef79d7c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1192,7 +1192,7 @@ int commander_thread_main(int argc, char *argv[]) * 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.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { -- cgit v1.2.3 From 5f752cdb7cf6c74e34b3ac79b9220c70172791e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 13:41:48 +0200 Subject: commander: Better audio indications on arming reject reasons --- src/modules/commander/commander.cpp | 6 +++--- src/modules/commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c26200048..0463cf8f8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1233,10 +1233,10 @@ int commander_thread_main(int argc, char *argv[]) sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("NOT ARMING: Press safety switch first."); + print_reject_arm("#audio: NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f1a353d5b..87309b238 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -138,7 +138,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Allow if HIL_STATE_ON if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first."); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); } valid_transition = false; @@ -312,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: /* we're in HIL and unexpected things can happen if we disable HIL now */ - mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); valid_transition = false; break; -- cgit v1.2.3 From a359a1b9408c7e4827c7dcbf7c27bf76ea071662 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 May 2014 15:02:24 +0200 Subject: Fixed audio output to provide decent user feedback --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0463cf8f8..5c0628a16 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1848,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED: - mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command); + /* this needs additional hints to the user - so let other messages pass and be spoken */ + mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command); tune_negative(true); break; -- cgit v1.2.3 From c7bf00562d685b513e1720f558fee5840aca151b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 08:22:54 +0200 Subject: commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail --- src/modules/commander/commander.cpp | 27 ++++++++++++++++++++++++++ src/modules/commander/state_machine_helper.cpp | 22 +++++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 51 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a16..55c74fdff 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -717,6 +718,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = hrt_absolute_time(); + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = -1.0f; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -853,6 +857,11 @@ int commander_thread_main(int argc, char *argv[]) struct position_setpoint_triplet_s pos_sp_triplet; memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + /* Subscribe to system power */ + int system_power_sub = orb_subscribe(ORB_ID(system_power)); + struct system_power_s system_power; + memset(&system_power, 0, sizeof(system_power)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -933,6 +942,24 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); } + orb_check(system_power_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), system_power_sub, &system_power); + + if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + if (system_power.servo_valid && + !system_power.brick_valid && + !system_power.usb_connected) { + /* flying only on servo rail, this is unsafe */ + status.condition_power_input_valid = false; + } else { + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = system_power.voltage5V_v; + } + } + } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); /* update safety topic */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..818974648 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,6 +144,28 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } + + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } + + valid_transition = false; + } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { new_arming_state = ARMING_STATE_STANDBY_ERROR; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ea20a317a..2ed0d6ad4 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -179,6 +179,8 @@ struct vehicle_status_s { 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 */ + bool condition_power_input_valid; /**< set if input power is valid */ + float avionics_power_rail_voltage; /**< voltage of the avionics power rail */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ -- cgit v1.2.3 From 064329dd099819b0f0b8a9a7bc0233440fdf6df1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:32 +0200 Subject: commander: put circuit breaker into effect --- src/modules/commander/commander.cpp | 7 ++++++ src/modules/commander/state_machine_helper.cpp | 32 +++++++++++++++----------- 2 files changed, 25 insertions(+), 14 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55c74fdff..71067ac4f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -721,6 +722,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = true; status.avionics_power_rail_voltage = -1.0f; + // CIRCUIT BREAKERS + status.circuit_breaker_engaged_power_check = false; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -907,6 +911,9 @@ 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.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status_changed = true; /* re-check RC calibration */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 818974648..84f4d03af 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,26 +144,30 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } - // Fail transition if power is not good - if (!status->condition_power_input_valid) { + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + valid_transition = false; } - valid_transition = false; - } + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { - // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + valid_transition = false; } - - valid_transition = false; } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From ed6c2a5168ca891f20594687acfd3c6bbf7e1cf9 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 May 2014 21:56:32 +0200 Subject: commander and navigator: lot's of changes, failsafe handling in commander, navigator only for execution (WIP) --- src/drivers/blinkm/blinkm.cpp | 5 +- src/drivers/gps/mtk.cpp | 4 +- src/modules/commander/commander.cpp | 472 ++++++++++----------- src/modules/commander/state_machine_helper.cpp | 414 ++++-------------- src/modules/commander/state_machine_helper.h | 16 +- src/modules/mavlink/mavlink_messages.cpp | 86 ++-- src/modules/navigator/navigator_main.cpp | 240 +---------- src/modules/systemlib/state_table.h | 28 +- src/modules/uORB/topics/mission_result.h | 1 + .../uORB/topics/position_setpoint_triplet.h | 15 +- src/modules/uORB/topics/vehicle_status.h | 35 +- 11 files changed, 411 insertions(+), 905 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..98c491ce6 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -655,13 +655,14 @@ BlinkM::led() /* indicate main control state */ if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) + /* TODO: add other Auto modes */ + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; - else + else led_color_4 = LED_OFF; led_color_5 = led_color_4; } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index ea4e95fb2..41716cd97 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -251,8 +251,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->lon = 0; // Indicate this data is not usable and bail out - _gps_position->eph_m = 1000.0f; - _gps_position->epv_m = 1000.0f; + _gps_position->eph = 1000.0f; + _gps_position->epv = 1000.0f; _gps_position->fix_type = 0; return; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 77d810a81..908384bb6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,11 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * 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 @@ -38,8 +33,13 @@ /** * @file commander.cpp - * Main system state machine implementation. + * Main fail-safe handling. * + * @author Petri Tanskanen + * @author Lorenz Meier + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin */ #include @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -87,6 +88,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -387,103 +389,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { - /* result of the command */ - enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } - /* only handle high-priority commands here */ + /* result of the command */ + enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t) cmd->param1; - uint8_t custom_main_mode = (uint8_t) cmd->param2; + 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; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); - } + transition_result_t arming_ret = TRANSITION_NOT_CHANGED; - // Transition the arming state - transition_result_t arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); + transition_result_t main_ret = TRANSITION_NOT_CHANGED; - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - if (hil_ret == OK) { - ret = true; - } - - // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - - if (arming_res == TRANSITION_CHANGED) { - ret = true; - } + // Transition the arming state + arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { - /* 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_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { + /* MANUAL */ + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_res = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status, MAIN_STATE_ACRO); } - } else { + } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } } } - } - if (main_res == TRANSITION_CHANGED) { - ret = true; - } + if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) { + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } else { + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } - break; - } case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. @@ -502,10 +481,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } } } @@ -516,14 +495,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_LOITER; + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_MISSION; + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -531,6 +510,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; +#if 0 /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command @@ -538,15 +518,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { /* reject parachute depoyment not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; +#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; @@ -560,10 +541,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } else { @@ -574,10 +555,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) { warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); @@ -609,13 +590,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); + answer_command(*cmd, cmd_result); } /* send any requested ACKs */ - if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } @@ -682,7 +663,7 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_NONE; + status.set_nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; status.failsafe_state = FAILSAFE_STATE_NORMAL; @@ -772,6 +753,11 @@ int commander_thread_main(int argc, char *argv[]) safety.safety_switch_available = false; safety.safety_off = false; + /* Subscribe to mission result topic */ + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -849,6 +835,13 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); + transition_result_t arming_ret; + + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = false; + bool main_state_changed = false; + bool failsafe_state_changed = false; + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { @@ -856,6 +849,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + arming_ret = TRANSITION_NOT_CHANGED; + + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -935,6 +931,7 @@ int commander_thread_main(int argc, char *argv[]) if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + arming_state_changed = true; } } } @@ -1131,12 +1128,21 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 1"); + arming_state_changed = true; + } } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); - } + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed 2"); + arming_state_changed = true; + } + } status_changed = true; } @@ -1144,11 +1150,18 @@ 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); + /* TODO: check for sensors */ + warnx("arming status1: %d", status.arming_state); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + warnx("arming status2: %d", status.arming_state); + + if (arming_ret == TRANSITION_CHANGED) { + warnx("changed"); + arming_state_changed = true; + } } else { - // XXX: Add emergency stuff if sensors are lost + /* TODO: Add emergency stuff if sensors are lost */ } @@ -1167,6 +1180,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + } + /* start RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ @@ -1184,11 +1203,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t arming_res; // store all transitions results here - - /* arm/disarm by RC */ - arming_res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && @@ -1199,7 +1213,11 @@ 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); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 1"); + arming_state_changed = true; + } stick_off_counter = 0; } else { @@ -1221,7 +1239,11 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + if (arming_ret == TRANSITION_CHANGED) { + warnx("stick 2"); + arming_state_changed = true; + } } stick_on_counter = 0; @@ -1234,23 +1256,25 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (arming_res == TRANSITION_CHANGED) { + if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + arming_state_changed = true; - } else if (arming_res == TRANSITION_DENIED) { + } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - } + + // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + // /* recover from failsafe */ + // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + // } /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1258,102 +1282,41 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); + main_state_changed = true; } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAVIGATION_STATE_RTL; - - - } else { - - /* LOITER switch */ - if (sp_man.loiter_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAVIGATION_STATE_LOITER; - - } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAVIGATION_STATE_MISSION; - } - } - } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } + } - if (armed.armed) { - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - - if (auto_res == TRANSITION_NOT_CHANGED) { - last_auto_state_valid = hrt_absolute_time(); - } - - /* still invalid state after the timeout interval, execute failsafe */ - if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (auto_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_MISSION; - } + if (armed.armed) { + /* if RC is lost, switch to RTL_RC or Termination unless a mission is running + * and not yet finished) */ + if (status.rc_signal_lost + && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; } else { - /* failsafe for manual modes */ - transition_result_t manual_res = TRANSITION_DENIED; - - if (!status.condition_landed) { - /* vehicle is not landed, try to perform RTL */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - } - - if (manual_res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (manual_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } else { - status.set_nav_state = NAVIGATION_STATE_RTL; - } - } - - } else { - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* reset failsafe when disarmed */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + status.failsafe_state = FAILSAFE_STATE_LAND; } } - } - // TODO remove this hack - /* flight termination in manual mode if assist switch is on POSCTL position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { - if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(armed.armed); - } + /* if the data link is lost, switch to RTL_DL or Termination */ + /* TODO: add this */ + + } else { + /* reset failsafe when disarmed */ + status.failsafe_state = FAILSAFE_STATE_NORMAL; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1369,11 +1332,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* 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 failsafe_state_changed = check_failsafe_state_changed(); - hrt_abstime t1 = hrt_absolute_time(); /* print new state */ @@ -1408,18 +1366,24 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } was_armed = armed.armed; + /* now set nav state according to failsafe and main state */ + set_nav_state(&status); + if (main_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + main_state_changed = false; } if (failsafe_state_changed) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + failsafe_state_changed = false; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1608,6 +1572,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; + warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL @@ -1648,14 +1613,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO); + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // else fallback to ALTCTL (POSCTL likely will not work too) - print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1675,85 +1639,93 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } void - set_control_mode() { - /* set vehicle_control_mode according to main state and failsafe state */ + /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; + /* TODO: check this */ control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator should act */ - bool navigator_enabled = false; - - switch (status.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (status.main_state) { - case MAIN_STATE_MANUAL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - break; + switch (status.set_nav_state) { + case NAVIGATION_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - case MAIN_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - default: - break; - } + case NAVIGATION_STATE_ALTCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_POSCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_RTL: - navigator_enabled = true; + case NAVIGATION_STATE_AUTO_MISSION: + case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_LAND: - navigator_enabled = true; + case NAVIGATION_STATE_LAND: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_TERMINATION: + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -1769,21 +1741,6 @@ set_control_mode() default: break; } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - - /* in failsafe LAND mode position may be not available */ - control_mode.flag_control_position_enabled = status.condition_local_position_valid; - control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; - - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - } } void @@ -1927,7 +1884,6 @@ 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)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6f0e9794a..214480079 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -59,30 +60,20 @@ #include "state_machine_helper.h" #include "commander_helper.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -static bool arming_state_changed = true; -static bool main_state_changed = true; -static bool failsafe_state_changed = true; - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation @@ -165,7 +156,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; - arming_state_changed = true; } } @@ -199,69 +189,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet } } -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 *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* transition may be denied even if requested the same state because conditions may be changed */ + /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { case MAIN_STATE_MANUAL: - ret = TRANSITION_CHANGED; - break; - case MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; case MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ + /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || (status->condition_local_altitude_valid || status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } - break; case MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } - break; - case MAIN_STATE_AUTO: - + case MAIN_STATE_AUTO_MISSION: + case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case MAIN_STATE_AUTO_RTL: + /* need global position and home position */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } break; - } + case MAIN_STATE_MAX: + default: + break; + } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { status->main_state = new_main_state; - main_state_changed = true; - } else { ret = TRANSITION_NOT_CHANGED; } @@ -270,70 +249,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } -bool -check_main_state_changed() -{ - if (main_state_changed) { - main_state_changed = false; - return true; - - } else { - return false; - } -} - -bool -check_failsafe_state_changed() -{ - if (failsafe_state_changed) { - failsafe_state_changed = false; - return true; - - } else { - return false; - } -} - /** -* Transition from one hil state to another -*/ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_transition = false; - int ret = ERROR; + transition_result_t ret = TRANSITION_DENIED; if (current_status->hil_state == new_state) { - valid_transition = true; + ret = TRANSITION_NOT_CHANGED; } else { - switch (new_state) { - case HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - valid_transition = false; - + ret = TRANSITION_DENIED; 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_ERROR) { - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - - // Disable publication of all attached sensors - + /* Disable publication of all attached sensors */ /* list directory */ DIR *d; d = opendir("/dev"); if (d) { - struct dirent *direntry; char devname[24]; @@ -388,288 +332,98 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } - closedir(d); + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + } else { /* failed opening dir */ - warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); - return 1; + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; } + } else { + mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + ret = TRANSITION_DENIED; } - break; default: - warnx("Unknown hil state"); + warnx("Unknown HIL state"); break; } } - if (valid_transition) { + if (ret = TRANSITION_CHANGED) { current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - // XXX also set lockdown here - - ret = OK; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } - return ret; } /** -* Transition from one failsafe state to another -*/ -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) + * Check failsafe and main status and set navigation status for navigator accordingly + */ +void set_nav_state(struct vehicle_status_s *status) { - transition_result_t ret = TRANSITION_DENIED; - - /* transition may be denied even if requested the same state because conditions may be changed */ - if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { - /* transitions from TERMINATION to other states not allowed */ - if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { - ret = TRANSITION_NOT_CHANGED; - } - - } else { - switch (new_failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* always allowed (except from TERMINATION state) */ - ret = TRANSITION_CHANGED; + switch (status->failsafe_state) { + case FAILSAFE_STATE_NORMAL: + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_MANUAL: + status->set_nav_state = NAVIGATION_STATE_MANUAL; break; - case FAILSAFE_STATE_RTL: - - /* global position and home position required for RTL */ - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAVIGATION_STATE_RTL; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_ALTCTL: + status->set_nav_state = NAVIGATION_STATE_ALTCTL; + break; + case MAIN_STATE_POSCTL: + status->set_nav_state = NAVIGATION_STATE_POSCTL; break; - case FAILSAFE_STATE_LAND: + case MAIN_STATE_AUTO_MISSION: + status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + break; - /* at least relative altitude estimate required for landing */ - if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAVIGATION_STATE_LAND; - ret = TRANSITION_CHANGED; - } + case MAIN_STATE_AUTO_LOITER: + status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + break; + case MAIN_STATE_AUTO_RTL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; break; - case FAILSAFE_STATE_TERMINATION: - /* always allowed */ - ret = TRANSITION_CHANGED; + case MAIN_STATE_ACRO: + status->set_nav_state = NAVIGATION_STATE_ACRO; break; default: break; } + break; - if (ret == TRANSITION_CHANGED) { - if (status->failsafe_state != new_failsafe_state) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; - - } else { - ret = TRANSITION_NOT_CHANGED; - } - } - } + case FAILSAFE_STATE_RTL_RC: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + break; - return ret; -} + case FAILSAFE_STATE_RTL_DL: + status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + break; + case FAILSAFE_STATE_LAND: + status->set_nav_state = NAVIGATION_STATE_LAND; + break; + case FAILSAFE_STATE_TERMINATION: + status->set_nav_state = NAVIGATION_STATE_TERMINATION; + break; -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ -// -//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -//{ -// int ret = 1; -// -//// /* Switch on HIL if in standby and not already in HIL mode */ -//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) -//// && !current_status->flag_hil_enabled) { -//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { -//// /* Enable HIL on request */ -//// current_status->flag_hil_enabled = true; -//// ret = OK; -//// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); -//// -//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_fmu_armed) { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") -//// -//// } else { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") -//// } -//// } -// -// /* switch manual / auto */ -// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { -// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { -// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { -// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { -// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); -// } -// -// /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only arm in standby state */ -// // XXX REMOVE -// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); -// ret = OK; -// printf("[cmd] arming due to command request\n"); -// } -// } -// -// /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only disarm in ground ready */ -// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -// ret = OK; -// printf("[cmd] disarming due to command request\n"); -// } -// } -// -// /* NEVER actually switch off HIL without reboot */ -// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { -// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); -// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); -// ret = ERROR; -// } -// -// return ret; -//} + default: + break; + } +} diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..594bf23a1 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,25 +56,17 @@ 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 int mavlink_fd = 0); - 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(); +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 int mavlink_fd = 0); 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 failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); -bool check_navigation_state_changed(); - -bool check_failsafe_state_changed(); - -void set_navigation_state_changed(); +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); +void set_nav_state(struct vehicle_status_s *status); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 70087cf3e..31c0c8f79 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,51 +118,71 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE_ON_GROUND - || pos_sp_triplet->nav_state == NAV_STATE_NONE_IN_AIR) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - - } else if (status->main_state == MAIN_STATE_ALTCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + switch (status->set_nav_state) { - } else if (status->main_state == MAIN_STATE_POSCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; - - } else if (status->main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; - } else if (status->main_state == MAIN_STATE_ACRO) { + case NAVIGATION_STATE_ACRO: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - } - - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + break; - if (pos_sp_triplet->nav_state == NAV_STATE_AUTO_ON_GROUND) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { + case NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + case NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; + + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTL_RC: + case NAVIGATION_STATE_AUTO_RTL_DL: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + case NAVIGATION_STATE_LAND: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } + break; + + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + } *mavlink_custom_mode = custom_mode.data; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 74694447a..f30cd9a94 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -32,7 +32,6 @@ ****************************************************************************/ /** * @file navigator_main.cpp - * Implementation of the main navigation state machine. * * Handles mission items, geo fencing and failsafe navigation behavior. * Published the position setpoint triplet for the position controller. @@ -75,7 +74,6 @@ #include #include -#include #include #include #include @@ -102,7 +100,7 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator : public StateTable +class Navigator { public: /** @@ -116,7 +114,7 @@ public: ~Navigator(); /** - * Start the navigator task. + * Start the navigator task. * * @return OK on success. */ @@ -200,24 +198,6 @@ private: param_t takeoff_alt; } _parameter_handles; /**< handles for parameters */ - enum Event { - EVENT_NONE_REQUESTED, - EVENT_LOITER_REQUESTED, - EVENT_MISSION_REQUESTED, - EVENT_RTL_REQUESTED, - EVENT_TAKEN_OFF, - EVENT_LANDED, - EVENT_MISSION_CHANGED, - EVENT_HOME_POSITION_CHANGED, - EVENT_MISSION_ITEM_REACHED, - MAX_EVENT - }; /**< possible events that can be thrown at state machine */ - - /** - * State machine transition table - */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - /** * Update our local parameter cache. */ @@ -268,41 +248,6 @@ private: */ void task_main(); - /** - * Functions that are triggered when a new state is entered. - */ - bool start_none_on_ground(); - bool start_none_in_air(); - bool start_auto_on_ground(); - bool start_loiter(); - bool start_mission(); - bool advance_mission(); - bool start_rtl(); - bool advance_rtl(); - bool start_land(); - - /** - * Reset all "mission item reached" flags. - */ - void reset_reached(); - - /** - * Check if current mission item has been reached. - */ - bool check_mission_item_reached(); - - /** - * Set mission items by accessing the mission class. - * If failing, tell the state machine to loiter. - */ - bool set_mission_items(); - - /** - * Set a RTL item by accessing the RTL class. - * If failing, tell the state machine to loiter. - */ - void set_rtl_item(); - /** * Translate mission item to a position setpoint. */ @@ -329,7 +274,6 @@ Navigator *g_navigator; Navigator::Navigator() : /* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), @@ -368,11 +312,6 @@ Navigator::Navigator() : _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - - /* Initialize state machine */ - myState = NAV_STATE_NONE_ON_GROUND; - - start_none_on_ground(); } Navigator::~Navigator() @@ -615,41 +554,6 @@ Navigator::task_main() /* vehicle status updated */ if (fds[6].revents & POLLIN) { vehicle_status_update(); - - /* commander requested new navigation mode, forward it to state machine */ - switch (_vstatus.set_nav_state) { - case NAVIGATION_STATE_NONE: - dispatch(EVENT_NONE_REQUESTED); - break; - - case NAVIGATION_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); - break; - - case NAVIGATION_STATE_MISSION: - dispatch(EVENT_MISSION_REQUESTED); - break; - - case NAVIGATION_STATE_RTL: - dispatch(EVENT_RTL_REQUESTED); - break; - - case NAVIGATION_STATE_LAND: - /* TODO: add and test this */ - // dispatch(EVENT_LAND_REQUESTED); - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - /* commander sets in-air/on-ground flag as well */ - if (_vstatus.condition_landed) { - dispatch(EVENT_LANDED); - } else { - dispatch(EVENT_TAKEN_OFF); - } } /* parameters updated */ @@ -666,30 +570,21 @@ Navigator::task_main() /* offboard mission updated */ if (fds[4].revents & POLLIN) { offboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* onboard mission updated */ if (fds[5].revents & POLLIN) { onboard_mission_update(); - /* TODO: check if mission really changed */ - dispatch(EVENT_MISSION_CHANGED); } /* home position updated */ if (fds[2].revents & POLLIN) { home_position_update(); - /* TODO check if home position really changed */ - dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ if (fds[1].revents & POLLIN) { global_position_update(); - if (check_mission_item_reached()) { - dispatch(EVENT_MISSION_ITEM_REACHED); - } /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { @@ -765,125 +660,8 @@ Navigator::status() } else { warnx("Geofence not set"); } - - switch (myState) { - case NAV_STATE_NONE_ON_GROUND: - warnx("State: None on ground"); - break; - - case NAV_STATE_NONE_IN_AIR: - warnx("State: None in air"); - break; - - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - - case NAV_STATE_LAND: - warnx("State: Land"); - break; - - default: - warnx("State: Unknown"); - break; - } } - -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* NAV_STATE_NONE_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_ON_GROUND}, - }, - { - /* NAV_STATE_NONE_IN_AIR */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_none_on_ground), NAV_STATE_NONE_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_NONE_IN_AIR}, - }, - { - /* NAV_STATE_AUTO_ON_GROUND */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_TAKEN_OFF */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_AUTO_ON_GROUND}, - }, - { - /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LOITER}, - }, - { - /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_mission), NAV_STATE_MISSION}, - }, - { - /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LANDED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_MISSION_ITEM_REACHED */ {ACTION(&Navigator::advance_rtl), NAV_STATE_RTL}, - }, - { - /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none_in_air), NAV_STATE_NONE_IN_AIR}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_TAKEN_OFF */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_LANDED */ {ACTION(&Navigator::start_auto_on_ground), NAV_STATE_AUTO_ON_GROUND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_ITEM_REACHED */ {NO_ACTION, NAV_STATE_LAND}, - }, -}; - +#if 0 bool Navigator::start_none_on_ground() { @@ -984,13 +762,11 @@ Navigator::set_mission_items() /* if we fail to set the current mission, continue to loiter */ if (!_mission.get_current_mission_item(&_mission_item, &onboard, &index)) { - dispatch(EVENT_LOITER_REQUESTED); return false; } /* if we got an RTL mission item, switch to RTL mode and give up */ if (_mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - dispatch(EVENT_RTL_REQUESTED); return false; } @@ -1042,7 +818,6 @@ Navigator::start_rtl() } /* if RTL doesn't work, fallback to loiter */ - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1066,7 +841,6 @@ Navigator::advance_rtl() return true; } - dispatch(EVENT_LOITER_REQUESTED); return false; } @@ -1105,7 +879,7 @@ Navigator::start_land() _update_triplet = true; return true; } - +#endif void Navigator::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { @@ -1147,7 +921,7 @@ Navigator::mission_item_to_position_setpoint(const mission_item_s *item, positio sp->type = SETPOINT_TYPE_NORMAL; } } - +#if 0 bool Navigator::check_mission_item_reached() { @@ -1251,12 +1025,12 @@ Navigator::reset_reached() _waypoint_yaw_reached = false; } - +#endif void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - _pos_sp_triplet.nav_state = static_cast(myState); + /* TODO: set nav_state */ /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index 38378651b..e6011fdef 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -33,7 +33,7 @@ /** * @file state_table.h - * + * * Finite-State-Machine helper class for state table * @author: Julian Oes */ @@ -44,32 +44,32 @@ class StateTable { public: - typedef bool (StateTable::*Action)(); + typedef void (StateTable::*Action)(); struct Tran { Action action; unsigned nextState; }; - + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) : myTable(table), myNsignals(nSignals), myNstates(nStates) {} - + #define NO_ACTION &StateTable::doNothing #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - + void dispatch(unsigned const sig) { /* get transition using state table */ Tran const *t = myTable + myState*myNsignals + sig; - /* first up change state, this allows to do further dispatchs in the state functions */ - - /* now execute state function, if it runs with success, accept new state */ - if ((this->*(t->action))()) { - myState = t->nextState; - } + + /* accept new state */ + myState = t->nextState; + + /* */ + (this->*(t->action))(); } - bool doNothing() { - return true; + void doNothing() { + return; } protected: unsigned myState; @@ -79,4 +79,4 @@ private: unsigned myNstates; }; -#endif \ No newline at end of file +#endif diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 7c3921198..ad654a9ff 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -56,6 +56,7 @@ struct mission_result_s bool mission_reached; /**< true if mission has been reached */ unsigned mission_index_reached; /**< index of the mission which has been reached */ unsigned index_current_mission; /**< index of the current mission */ + bool mission_finished; /**< true if mission has been completed */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 85e8ef8a5..c2741a05b 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -74,14 +74,13 @@ struct position_setpoint_s }; typedef enum { - NAV_STATE_NONE_ON_GROUND = 0, - NAV_STATE_NONE_IN_AIR, - NAV_STATE_AUTO_ON_GROUND, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX + NAV_STATE_MANUAL = 0, + NAV_STATE_POSCTL, + NAV_STATE_AUTO, + NAV_STATE_RC_LOSS, + NAV_STATE_DL_LOSS, + NAV_STATE_TERMINATION, + MAX_NAV_STATE } nav_state_t; /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index d902dc49e..259d7329e 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,9 +63,11 @@ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, MAIN_STATE_POSCTL, - MAIN_STATE_AUTO, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, MAIN_STATE_ACRO, - MAIN_STATE_MAX + MAIN_STATE_MAX, } main_state_t; // If you change the order, add or remove arming_state_t states make sure to update the arrays @@ -78,7 +80,7 @@ typedef enum { ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE, - ARMING_STATE_MAX + ARMING_STATE_MAX, } arming_state_t; typedef enum { @@ -88,18 +90,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL, /**< Return To Launch */ - FAILSAFE_STATE_LAND, /**< Land without position control */ + FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX + FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_NONE = 0, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND + NAVIGATION_STATE_MANUAL = 0, + NAVIGATION_STATE_ACRO, + NAVIGATION_STATE_ALTCTL, + NAVIGATION_STATE_POSCTL, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_RTL_RC, + NAVIGATION_STATE_AUTO_RTL_DL, + NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TERMINATION, } navigation_state_t; enum VEHICLE_MODE_FLAG { @@ -160,10 +169,10 @@ 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 */ - main_state_t main_state; /**< main state machine */ + main_state_t main_state; /**< main state machine */ navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ - hil_state_t hil_state; /**< current hil state */ + hil_state_t hil_state; /**< current hil state */ failsafe_state_t failsafe_state; /**< current failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ -- cgit v1.2.3 From c8903b167230bc2efdb908e78135a0fa0abc745c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 27 May 2014 23:24:01 +0200 Subject: commander: modes and RC loss working now --- src/modules/commander/commander.cpp | 69 ++++++++++++++++++++----------------- 1 file changed, 37 insertions(+), 32 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 908384bb6..5638bc09f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -625,8 +625,10 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[0] = "MANUAL"; main_states_str[1] = "ALTCTL"; main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO"; - main_states_str[4] = "ACRO"; + main_states_str[3] = "AUTO_MISSION"; + main_states_str[4] = "AUTO_LOITER"; + main_states_str[5] = "AUTO_RTL"; + main_states_str[6] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -639,9 +641,10 @@ int commander_thread_main(int argc, char *argv[]) char *failsafe_states_str[FAILSAFE_STATE_MAX]; failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL"; - failsafe_states_str[2] = "LAND"; - failsafe_states_str[3] = "TERMINATION"; + failsafe_states_str[1] = "RTL_RC"; + failsafe_states_str[2] = "RTL_DL"; + failsafe_states_str[3] = "LAND"; + failsafe_states_str[4] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -1151,12 +1154,9 @@ 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) { /* TODO: check for sensors */ - warnx("arming status1: %d", status.arming_state); arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); - warnx("arming status2: %d", status.arming_state); if (arming_ret == TRANSITION_CHANGED) { - warnx("changed"); arming_state_changed = true; } @@ -1198,6 +1198,9 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; + + status.failsafe_state = FAILSAFE_STATE_NORMAL; + failsafe_state_changed = true; } } @@ -1215,7 +1218,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed); if (arming_ret == TRANSITION_CHANGED) { - warnx("stick 1"); arming_state_changed = true; } stick_off_counter = 0; @@ -1241,7 +1243,6 @@ int commander_thread_main(int argc, char *argv[]) } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); if (arming_ret == TRANSITION_CHANGED) { - warnx("stick 2"); arming_state_changed = true; } } @@ -1294,29 +1295,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - } - } - if (armed.armed) { - /* if RC is lost, switch to RTL_RC or Termination unless a mission is running - * and not yet finished) */ - if (status.rc_signal_lost - && !(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; + if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; + } else { + status.failsafe_state = FAILSAFE_STATE_LAND; + } + failsafe_state_changed = true; } } - - /* if the data link is lost, switch to RTL_DL or Termination */ - /* TODO: add this */ - - } else { - /* reset failsafe when disarmed */ - status.failsafe_state = FAILSAFE_STATE_NORMAL; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1613,10 +1604,24 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + if (sp_man->return_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_RTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else { + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } // else fallback to ALTCTL (POSCTL likely will not work too) -- cgit v1.2.3 From 480c41d83591354480a8fd81cfbc314fccaa6a20 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 1 Jun 2014 13:18:56 +0200 Subject: do not read out home position in gcs (home position is still displayed) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a16..65922b2a5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1408,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[]) home.alt = global_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); - mavlink_log_info(mavlink_fd, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); + mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); /* announce new home position */ if (home_pub > 0) { -- cgit v1.2.3 From 1fc859b1b195b587e70e4a8f6fbeb952424d0610 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Mon, 2 Jun 2014 23:11:31 -0400 Subject: Added support for postion_control offboard messages in commander. --- src/modules/commander/commander.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 90efd5115..29a5f0b4a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1809,6 +1809,14 @@ set_control_mode() control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = false; + break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; -- cgit v1.2.3 From e12d3af503c5ebc105e5b3c3ef8e51bd75959329 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Tue, 3 Jun 2014 23:38:18 -0400 Subject: Added support for offboard velocity control & fixed position control flags --- src/modules/commander/commander.cpp | 22 +++++++++++++++------- 1 file changed, 15 insertions(+), 7 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 29a5f0b4a..3da43a792 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1809,12 +1809,12 @@ set_control_mode() control_mode.flag_control_offboard_enabled = true; switch (sp_offboard.mode) { - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = false; + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = true; + control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -1825,13 +1825,21 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; break; default: control_mode.flag_control_rates_enabled = false; -- cgit v1.2.3 From 94c4fc56aa229bcb909e5437804e7475adfbcdba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 6 Jun 2014 23:08:11 +0200 Subject: navigator: audio messages about what is happening, RTL during mission not triggered but after mission --- src/modules/commander/commander.cpp | 17 ++++++++++- src/modules/navigator/mission.cpp | 60 +++++++++++++++++++++++++++++-------- src/modules/navigator/mission.h | 5 ++++ src/modules/navigator/rtl.cpp | 6 +--- 4 files changed, 70 insertions(+), 18 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d7a95b0d6..bb75b2af0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1296,7 +1296,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ @@ -1306,10 +1305,26 @@ int commander_thread_main(int argc, char *argv[]) status.failsafe_state = FAILSAFE_STATE_LAND; } failsafe_state_changed = true; + } else { + mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); } } } + /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ + if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + /* if we have a global position, we can switch to RTL, if not, we can try to land */ + if (status.condition_global_position_valid) { + status.failsafe_state = FAILSAFE_STATE_RTL_RC; + mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); + } else { + /* this probably doesn't make sense since we are in mission and have global position */ + status.failsafe_state = FAILSAFE_STATE_LAND; + } + failsafe_state_changed = true; + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cd694be9a..db606e6fa 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -46,6 +46,7 @@ #include #include +#include #include #include @@ -210,20 +211,39 @@ Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) { set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); + /* try setting onboard mission item */ if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting onboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_ONBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: onboard mission running"); + } _mission_type = MISSION_TYPE_ONBOARD; _navigator->set_is_in_loiter(false); + /* try setting offboard mission item */ } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { - /* try setting offboard mission item */ + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_OFFBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: offboard mission running"); + } _mission_type = MISSION_TYPE_OFFBOARD; _navigator->set_is_in_loiter(false); } else { + if (_mission_type != MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: mission finished"); + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: no mission available"); + } _mission_type = MISSION_TYPE_NONE; bool use_current_pos_sp = pos_sp_triplet->current.valid && _waypoint_position_reached; set_loiter_item(use_current_pos_sp, pos_sp_triplet); + reset_mission_item_reached(); + report_mission_finished(); } } @@ -242,8 +262,9 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current { /* make sure param is up to date */ updateParams(); - if (_param_onboard_enabled.get() > 0 - && _current_onboard_mission_index < (int)_onboard_mission.count) { + if (_param_onboard_enabled.get() > 0 && + _current_onboard_mission_index >= 0&& + _current_onboard_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, &new_mission_item)) { @@ -264,7 +285,8 @@ Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current bool Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - if (_current_offboard_mission_index < (int)_offboard_mission.count) { + if (_current_offboard_mission_index >= 0 && + _current_offboard_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -282,8 +304,6 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren report_current_offboard_mission_item(); memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); return true; - } else { - warnx("ERROR: WP read fail"); } } return false; @@ -295,7 +315,8 @@ Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) int next_temp_mission_index = _onboard_mission.current_index + 1; /* try if there is a next onboard mission */ - if (next_temp_mission_index < (int)_onboard_mission.count) { + if (_onboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { /* convert next mission item to position setpoint */ @@ -315,7 +336,9 @@ Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ int next_temp_mission_index = _offboard_mission.current_index + 1; - if (next_temp_mission_index < (int)_offboard_mission.count) { + warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); + if (_offboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; @@ -346,14 +369,17 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* read mission item from datamanager */ if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR waypoint could not be read"); return false; } /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - /* TODO: do this check more gracefully since it is not a serious error */ if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: DO JUMP repetitions completed"); return false; } @@ -366,9 +392,10 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { /* not supposed to happen unless the datamanager can't access the dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); return false; } - /* TODO: report about DO JUMP count */ } /* set new mission item index and repeat * we don't have to validate here, if it's invalid, we should realize this later .*/ @@ -381,7 +408,8 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio } /* we have given up, we don't want to cycle forever */ - warnx("ERROR: cycling through mission items without success"); + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP is cycling, giving up"); return false; } @@ -402,6 +430,13 @@ Mission::report_current_offboard_mission_item() publish_mission_result(); } +void +Mission::report_mission_finished() +{ + _mission_result.mission_finished = true; + publish_mission_result(); +} + void Mission::publish_mission_result() { @@ -416,4 +451,5 @@ Mission::publish_mission_result() } /* reset reached bool */ _mission_result.mission_reached = false; + _mission_result.mission_finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index cb3242c87..a3dd09ecd 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -151,6 +151,11 @@ private: */ void report_current_offboard_mission_item(); + /** + * Report that the mission is finished if one exists or that none exists + */ + void report_mission_finished(); + /** * Publish the mission result so commander and mavlink know what is going on */ diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index c4c5f2aab..dde35d5b6 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -112,6 +112,7 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) /* for safety reasons don't go into RTL if landed */ if (_navigator->get_vstatus()->condition_landed) { _rtl_state = RTL_STATE_FINISHED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); /* if lower than return altitude, climb up first */ } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { @@ -133,11 +134,6 @@ RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - /* TODO understand and fix this */ - // if (_vstatus.condition_landed) { - // climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - // } - _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; _mission_item.altitude_is_relative = false; -- cgit v1.2.3 From d48a8bc073a3aa5f515c582a1c2c3cae58a8d783 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 12 Jun 2014 19:09:18 +0200 Subject: navigator: renamed the different RTL states --- src/modules/commander/commander.cpp | 10 +++---- src/modules/commander/state_machine_helper.cpp | 8 +++--- src/modules/mavlink/mavlink_messages.cpp | 4 +-- src/modules/navigator/navigator_main.cpp | 4 +-- src/modules/uORB/topics/vehicle_status.h | 37 +++++++++++++------------- 5 files changed, 32 insertions(+), 31 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb75b2af0..e992706ac 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1300,7 +1300,7 @@ int commander_thread_main(int argc, char *argv[]) /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; } else { status.failsafe_state = FAILSAFE_STATE_LAND; } @@ -1313,10 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RTL_RC) { + mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RTL_RC; + status.failsafe_state = FAILSAFE_STATE_RC_LOSS; mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); } else { /* this probably doesn't make sense since we are in mission and have global position */ @@ -1719,8 +1719,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 214480079..c52e618ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -406,12 +406,12 @@ void set_nav_state(struct vehicle_status_s *status) } break; - case FAILSAFE_STATE_RTL_RC: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_RC; + case FAILSAFE_STATE_RC_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; break; - case FAILSAFE_STATE_RTL_DL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL_DL; + case FAILSAFE_STATE_DL_LOSS: + status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; break; case FAILSAFE_STATE_LAND: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index e7d2ba515..b58fb4cb8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -161,8 +161,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index dc8573813..20cff5c28 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,8 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_RTL_RC: - case NAVIGATION_STATE_AUTO_RTL_DL: + case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: + case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 259d7329e..9390ff717 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Petri Tanskanen - * @author Thomas Gubler - * @author Julian Oes + * Copyright (C) 2012 - 2014 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 @@ -45,6 +41,11 @@ * All apps should write to subsystem_info: * * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + * + * @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes */ #ifndef VEHICLE_STATUS_H_ @@ -90,25 +91,25 @@ typedef enum { typedef enum { FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL_RC, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_RTL_DL, /**< Return To Launch on datalink loss */ + FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ + FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ FAILSAFE_STATE_LAND, /**< Land as safe as possible */ FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ FAILSAFE_STATE_MAX, } failsafe_state_t; typedef enum { - NAVIGATION_STATE_MANUAL = 0, - NAVIGATION_STATE_ACRO, - NAVIGATION_STATE_ALTCTL, - NAVIGATION_STATE_POSCTL, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_RTL_RC, - NAVIGATION_STATE_AUTO_RTL_DL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TERMINATION, + NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ + NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ + NAVIGATION_STATE_POSCTL, /**< Position control mode */ + NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ + NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ + NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ + NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; enum VEHICLE_MODE_FLAG { -- cgit v1.2.3 From 91b590ef584cfc67be7555e3d7272bb94bc9b2b4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 13 Jun 2014 23:40:48 +0200 Subject: Move MISSION_STATE read/write from mavlink to navigator and commander --- src/modules/commander/commander.cpp | 22 ++++-- src/modules/dataman/dataman.h | 12 +--- src/modules/mavlink/mavlink_main.cpp | 119 ++++++------------------------- src/modules/navigator/mission.cpp | 33 +++++++++ src/modules/navigator/mission.h | 5 ++ src/modules/navigator/navigator_main.cpp | 7 -- 6 files changed, 80 insertions(+), 118 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..d2f8c2994 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -89,6 +90,7 @@ #include #include #include +#include #include "px4_custom_mode.h" #include "commander_helper.h" @@ -691,6 +693,11 @@ int commander_thread_main(int argc, char *argv[]) /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); + if (status_pub < 0) { + warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); + warnx("exiting."); + exit(ERROR); + } /* armed topic */ orb_advert_t armed_pub; @@ -708,10 +715,17 @@ int commander_thread_main(int argc, char *argv[]) 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); + /* init mission state */ + mission_s mission; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { + if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { + mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", + mission.dataman_id, mission.count, mission.current_index); + orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + close(mission_pub); + } else { + mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + } } mavlink_log_info(mavlink_fd, "[cmd] started"); diff --git a/src/modules/dataman/dataman.h b/src/modules/dataman/dataman.h index 215ec4c76..dbace21ef 100644 --- a/src/modules/dataman/dataman.h +++ b/src/modules/dataman/dataman.h @@ -53,7 +53,7 @@ extern "C" { DM_KEY_WAYPOINTS_OFFBOARD_0, /* Mission way point coordinates sent over mavlink */ DM_KEY_WAYPOINTS_OFFBOARD_1, /* (alernate between 0 and 1) */ DM_KEY_WAYPOINTS_ONBOARD, /* Mission way point coordinates generated onboard */ - DM_KEY_MISSION_STATE, /* Persistent mission state */ + DM_KEY_MISSION_STATE, /* Persistent mission state */ DM_KEY_NUM_KEYS /* Total number of item types defined */ } dm_item_t; @@ -127,16 +127,6 @@ extern "C" { dm_reset_reason restart_type /* The last reset type */ ); - /* NOTE: The following structure defines the persistent system state data stored in the single - entry DM_KEY_SYSTEM_STATE_KEY item type. It contains global system state information that - needs to survive restarts. This definition is application specific so it doesn't really belong - in this header, but till I find it a better home here it is */ - - typedef struct { - char offboard_waypoint_id; /* the index of the active offboard waypoint data */ - /* (DM_KEY_WAYPOINTS_OFFBOARD_n) or -1 for none */ - } persistent_system_state_t; - #ifdef __cplusplus } #endif diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 5c6ef68ee..bc4376b84 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -18,7 +18,6 @@ * * 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, @@ -964,8 +963,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) { - persistent_system_state_t sys_state; - /* init WPM state */ state->size = 0; state->max_size = MAVLINK_WPM_MAX_WP_COUNT; @@ -980,40 +977,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - int sys_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)); - if (sys_state_size == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) { - state->dataman_id = sys_state.offboard_waypoint_id; - - if (_verbose) { warnx("WPM init: using dataman ID %d", state->dataman_id); } - - /* count waypoints in current waypoints storage */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - int seq = 0; - while (seq < state->max_size && dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - seq++; - } - - if (_verbose) { warnx("WPM init: found %d items", seq); } - - state->size = seq; - - } else { - if (_verbose) { warnx("WPM init: invalid dataman ID %d stored in MISSION_STATE", sys_state.offboard_waypoint_id); } - } - - } else { - if (_verbose) { warnx("WPM init: dataman MISSION_STATE item has wrong size (%i, should be %u), ignoring", sys_state_size, sizeof(sys_state)); } - } - - /* init mission topic */ - mission.count = state->size; - mission.dataman_id = state->dataman_id; - mission.current_index = -1; // TODO store current index in dataman? - - publish_mission(); + } /* @@ -1081,10 +1045,10 @@ void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uin void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) { - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; struct mission_item_s mission_item; - if (dm_read(dm_id, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { /* create mission_item_s from mavlink_mission_item_t */ mavlink_mission_item_t wp; @@ -1103,7 +1067,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, dm_id); } + if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } } } @@ -1214,9 +1178,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) mission.current_index = wpc.seq; publish_mission(); - /* don't answer yet, wait for the navigator to respond, then publish the mission_result */ -// mavlink_wpm_send_waypoint_current(wpc.seq); - } else { if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } @@ -1392,6 +1353,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_count = wpc.count; _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + mission.current_index = -1; + mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } @@ -1442,10 +1405,10 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } - dm_item_t dm_id = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - if (dm_write(dm_id, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, dm_id); } + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); @@ -1453,6 +1416,11 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) break; } + /* waypoint marked as current */ + if (wp.current) { + mission.current_index = wp.seq; + } + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } _wpm->current_wp_id = wp.seq + 1; @@ -1463,65 +1431,23 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - /* write terminator item */ - if (_wpm->current_count < _wpm->max_size) { - if (dm_write(dm_id, _wpm->current_count, DM_PERSIST_POWER_ON_RESET, nullptr, 0) != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR writing terminator item"); } + mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - break; - } - } + /* update WPM state */ + _wpm->dataman_id = _wpm->current_dataman_id; + _wpm->size = _wpm->current_count; - /* offboard mission data saved correctly, now update the persistent system state */ - persistent_system_state_t state; - bool dm_result; + /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; + mission.count = _wpm->current_count; - /* since we are doing a read-modify-write we must lock the item type */ - dm_lock(DM_KEY_MISSION_STATE); - - /* first read in the current state data, there may eventually be data other than the offboard index - * and we must preserve it */ - ssize_t dm_state_size = dm_read(DM_KEY_MISSION_STATE, 0, &state, sizeof(state)); - if (dm_state_size != sizeof(state)) { - warnx("dataman MISSION_STATE size invalid: %d, should be %d", dm_state_size, sizeof(state)); - } - - /* set new dataman storage ID */ - state.offboard_waypoint_id = _wpm->current_dataman_id; - - /* write back to dataman */ - dm_result = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &state, sizeof(state)) != sizeof(state); - - dm_unlock(DM_KEY_MISSION_STATE); - - if (dm_result) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR unable to write storage ID = %d to dataman MISSION_STATE", state.offboard_waypoint_id); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - mission.dataman_id = _wpm->dataman_id; - mission.count = _wpm->current_count; - mission.current_index = 0; - - publish_mission(); - } + publish_mission(); } else { /* request next item */ mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); } } - break; } @@ -1545,6 +1471,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) _wpm->size = 0; /* update mission topic */ + mission.dataman_id = _wpm->dataman_id; mission.count = 0; mission.current_index = -1; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9244063b1..d9d8353f6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -430,6 +430,37 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio return false; } +void +Mission::save_offboard_mission_state() +{ + mission_s mission_state; + + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + /* read current state */ + int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); + + /* check if state actually changed to save flash write cycles */ + if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || + mission_state.count != _offboard_mission.count || + mission_state.current_index != _current_offboard_mission_index) { + + mission_state.dataman_id = _offboard_mission.dataman_id; + mission_state.count = _offboard_mission.count; + mission_state.current_index = _current_offboard_mission_index; + + /* write modifyed state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + + } + } + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); +} + void Mission::report_mission_item_reached() { @@ -445,6 +476,8 @@ Mission::report_current_offboard_mission_item() { _mission_result.index_current_mission = _current_offboard_mission_index; publish_mission_result(); + + save_offboard_mission_state(); } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 322aaf96a..98b11bade 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -141,6 +141,11 @@ private: bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item); + /** + * Save current offboard mission state to dataman + */ + void save_offboard_mission_state(); + /** * Report that a mission item has been reached */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 888c8e7f4..a3c190c7f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -227,13 +227,6 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* Get the last offboard mission id */ - persistent_system_state_t sys_state; - if (dm_read(DM_KEY_MISSION_STATE, 0, &sys_state, sizeof(sys_state)) == sizeof(sys_state)) { - if ((sys_state.offboard_waypoint_id >= 0) && (sys_state.offboard_waypoint_id <= 1)) - _mission.set_offboard_dataman_id(sys_state.offboard_waypoint_id); - } - /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); -- cgit v1.2.3 From 6cf890b46b5a112ed20b3f8ed7be7b658d5c4a8c Mon Sep 17 00:00:00 2001 From: andre-nguyen Date: Sat, 14 Jun 2014 15:27:07 -0400 Subject: indentation and fix commander flags. It's impossible to control position at the same time as attitude so we have to disable some things. My logic is that all the control flags for position control should be opposite of the attitude control mode. --- src/modules/commander/commander.cpp | 4 ++-- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3da43a792..2c36b2e6c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1834,8 +1834,8 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 9d9d2a7c0..6721d017d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -679,12 +679,12 @@ MulticopterPositionControl::task_main() /* Make sure position control is selected i.e. not only velocity control */ if (_control_mode.flag_control_position_enabled) { - _pos_sp(0) = _local_pos_sp.x; - _pos_sp(1) = _local_pos_sp.y; + _pos_sp(0) = _local_pos_sp.x; + _pos_sp(1) = _local_pos_sp.y; } if (_control_mode.flag_control_altitude_enabled) { - _pos_sp(2) = _local_pos_sp.z; + _pos_sp(2) = _local_pos_sp.z; } _att_sp.yaw_body = _local_pos_sp.yaw; -- cgit v1.2.3 From 5be741607c0d8d477ff30c7639edbd3bce427e7b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 14 Jun 2014 23:57:29 +0200 Subject: mavlink: mission manager moved to separate class and reworked --- src/modules/commander/commander.cpp | 25 +- src/modules/mavlink/mavlink_main.cpp | 766 +++------------------------- src/modules/mavlink/mavlink_main.h | 92 +--- src/modules/mavlink/mavlink_mission.cpp | 823 +++++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_mission.h | 176 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 13 +- src/modules/mavlink/mavlink_receiver.h | 2 - src/modules/mavlink/module.mk | 1 + src/modules/mavlink/util.h | 53 -- src/modules/mavlink/waypoints.h | 111 ----- src/modules/navigator/mission.cpp | 107 ++-- src/modules/navigator/mission.h | 2 + src/modules/uORB/topics/mission.h | 6 +- src/modules/uORB/topics/mission_result.h | 8 +- 14 files changed, 1168 insertions(+), 1017 deletions(-) create mode 100644 src/modules/mavlink/mavlink_mission.cpp create mode 100644 src/modules/mavlink/mavlink_mission.h delete mode 100644 src/modules/mavlink/util.h delete mode 100644 src/modules/mavlink/waypoints.h (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d2f8c2994..379ce45fb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -715,17 +715,27 @@ int commander_thread_main(int argc, char *argv[]) struct home_position_s home; memset(&home, 0, sizeof(home)); - /* init mission state */ + /* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */ + orb_advert_t mission_pub = -1; mission_s mission; if (dm_read(DM_KEY_MISSION_STATE, 0, &mission, sizeof(mission_s)) == sizeof(mission_s)) { if (mission.dataman_id >= 0 && mission.dataman_id <= 1) { - mavlink_log_info(mavlink_fd, "[cmd] dataman ID: %i, count: %u, current: %i", - mission.dataman_id, mission.count, mission.current_index); - orb_advert_t mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); - close(mission_pub); + warnx("loaded mission state: dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); + mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", + mission.dataman_id, mission.count, mission.current_seq); } else { + warnx("reading mission state failed"); mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + + /* initialize mission state in dataman */ + mission.dataman_id = 0; + mission.count = 0; + mission.current_seq = 0; + dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); } + + mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } mavlink_log_info(mavlink_fd, "[cmd] started"); @@ -1310,7 +1320,7 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = true; status_changed = true; - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { + if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.finished)) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { @@ -1327,7 +1337,7 @@ int commander_thread_main(int argc, char *argv[]) /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { + mission_result.finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { /* if we have a global position, we can switch to RTL, if not, we can try to land */ if (status.condition_global_position_valid) { status.failsafe_state = FAILSAFE_STATE_RC_LOSS; @@ -1488,6 +1498,7 @@ int commander_thread_main(int argc, char *argv[]) close(diff_pres_sub); close(param_changed_sub); close(battery_sub); + close(mission_pub); thread_running = false; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 126c4dfc6..f8a31a4ad 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -102,6 +102,8 @@ static struct file_operations fops; */ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]); +extern mavlink_system_t mavlink_system; + static uint64_t last_write_success_times[6] = {0}; static uint64_t last_write_try_times[6] = {0}; @@ -200,9 +202,6 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } } - - - } static void usage(void); @@ -221,6 +220,7 @@ Mavlink::Mavlink() : _main_loop_delay(1000), _subscriptions(nullptr), _streams(nullptr), + _mission_result_sub(-1), _mission_pub(-1), _mode(MAVLINK_MODE_NORMAL), _total_counter(0), @@ -238,12 +238,11 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _mission_manager(nullptr), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) { - _wpm = &_wpm_s; - fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; _instance_id = Mavlink::instance_count(); @@ -421,7 +420,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self) } void -Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self) +Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self) { Mavlink *inst; @@ -497,7 +496,6 @@ Mavlink::mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg) void Mavlink::mavlink_update_system(void) { - if (!_param_initialized) { _param_system_id = param_find("MAV_SYS_ID"); _param_component_id = param_find("MAV_COMP_ID"); @@ -701,9 +699,32 @@ Mavlink::set_hil_enabled(bool hil_enabled) return ret; } -extern mavlink_system_t mavlink_system; +void +Mavlink::send_message(const mavlink_message_t *msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; -int Mavlink::mavlink_pm_queued_send() + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + mavlink_send_uart_bytes(_channel, buf, len); +} + +void +Mavlink::handle_message(const mavlink_message_t *msg) +{ + /* handle packet with mission manager */ + _mission_manager->handle_message(msg); + + /* handle packet with parameter component */ + mavlink_pm_message_handler(_channel, msg); + + if (get_forwarding_on()) { + /* forward any messages to other mavlink instances */ + Mavlink::forward_message(msg, this); + } +} + +int +Mavlink::mavlink_pm_queued_send() { if (_mavlink_param_queue_index < param_count()) { mavlink_pm_send_param(param_for_index(_mavlink_param_queue_index)); @@ -780,7 +801,7 @@ int Mavlink::mavlink_pm_send_param(param_t param) mavlink_type, param_count(), param_get_index(param)); - mavlink_missionlib_send_message(&tx_msg); + send_message(&tx_msg); return OK; } @@ -794,7 +815,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); - mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); + send_statustext("[mavlink pm] sending list"); } } break; @@ -818,7 +839,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); - mavlink_missionlib_send_gcs_string(buf); + send_statustext(buf); } else { /* set and send parameter */ @@ -854,677 +875,18 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav } } -void Mavlink::publish_mission() -{ - /* Initialize mission publication if necessary */ - if (_mission_pub < 0) { - _mission_pub = orb_advertise(ORB_ID(offboard_mission), &_mission); - - } else { - orb_publish(ORB_ID(offboard_mission), _mission_pub, &_mission); - } -} - -int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) -{ - /* only support global waypoints for now */ - switch (mavlink_mission_item->frame) { - case MAV_FRAME_GLOBAL: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = false; - break; - - case MAV_FRAME_GLOBAL_RELATIVE_ALT: - mission_item->lat = (double)mavlink_mission_item->x; - mission_item->lon = (double)mavlink_mission_item->y; - mission_item->altitude = mavlink_mission_item->z; - mission_item->altitude_is_relative = true; - break; - - case MAV_FRAME_LOCAL_NED: - case MAV_FRAME_LOCAL_ENU: - return MAV_MISSION_UNSUPPORTED_FRAME; - - case MAV_FRAME_MISSION: - default: - return MAV_MISSION_ERROR; - } - - switch (mavlink_mission_item->command) { - case MAV_CMD_NAV_TAKEOFF: - mission_item->pitch_min = mavlink_mission_item->param1; - break; - case MAV_CMD_DO_JUMP: - mission_item->do_jump_mission_index = mavlink_mission_item->param1; - mission_item->do_jump_repeat_count = mavlink_mission_item->param2; - break; - default: - mission_item->acceptance_radius = mavlink_mission_item->param2; - mission_item->time_inside = mavlink_mission_item->param1; - break; - } - - mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); - mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); - mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ - mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; - - mission_item->autocontinue = mavlink_mission_item->autocontinue; - // mission_item->index = mavlink_mission_item->seq; - mission_item->origin = ORIGIN_MAVLINK; - - /* reset DO_JUMP count */ - mission_item->do_jump_current_count = 0; - - return OK; -} - -int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) -{ - if (mission_item->altitude_is_relative) { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL; - - } else { - mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; - } - - switch (mission_item->nav_cmd) { - case NAV_CMD_TAKEOFF: - mavlink_mission_item->param1 = mission_item->pitch_min; - break; - - case NAV_CMD_DO_JUMP: - mavlink_mission_item->param1 = mission_item->do_jump_mission_index; - mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; - break; - - default: - mavlink_mission_item->param2 = mission_item->acceptance_radius; - mavlink_mission_item->param1 = mission_item->time_inside; - break; - } - - mavlink_mission_item->x = (float)mission_item->lat; - mavlink_mission_item->y = (float)mission_item->lon; - mavlink_mission_item->z = mission_item->altitude; - - mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; - mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; - mavlink_mission_item->command = mission_item->nav_cmd; - mavlink_mission_item->autocontinue = mission_item->autocontinue; - // mavlink_mission_item->seq = mission_item->index; - - return OK; -} - -void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state) -{ - /* get offboard_mission topic */ - int mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - if (orb_copy(ORB_ID(offboard_mission), mission_sub, &_mission)) { - /* error getting topic, init to safe values */ - _mission.dataman_id = 0; - _mission.count = 0; - _mission.current_index = -1; - } - close(mission_sub); - - /* init WPM state */ - state->size = _mission.count; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->dataman_id = _mission.dataman_id; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->current_dataman_id = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timestamp_last_send_request = 0; - state->action_timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->retry_timeout = MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT; - - warnx("wpm init: dataman_id=%d, count=%u, current=%d", state->dataman_id, state->size, _mission.current_index); -} - -/* - * @brief Sends an waypoint ack message - */ -void Mavlink::mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_mission_ack_t wpa; - - wpa.target_system = sysid; - wpa.target_component = compid; - wpa.type = type; - - mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpa); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void Mavlink::mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if (seq < _wpm->size) { - mavlink_message_t msg; - mavlink_mission_current_t wpc; - - wpc.seq = seq; - - mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - } else if (seq == 0 && _wpm->size == 0) { - /* don't broadcast if no WPs */ - - } else { - if (_verbose) { warnx("WPM: Send WAYPOINT_CURRENT ERROR: seq %u out of bounds", seq); } - - mavlink_missionlib_send_gcs_string("ERROR: wp index out of bounds"); - } -} - -void Mavlink::mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_mission_count_t wpc; - - wpc.target_system = sysid; - wpc.target_component = compid; - wpc.count = _wpm->size; - - mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpc); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_COUNT %u to ID %u", wpc.count, wpc.target_system); } -} - -void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - dm_item_t dm_item = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - struct mission_item_s mission_item; - - if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { - - /* create mission_item_s from mavlink_mission_item_t */ - mavlink_mission_item_t wp; - map_mission_item_to_mavlink_mission_item(&mission_item, &wp); - - mavlink_message_t msg; - wp.target_system = sysid; - wp.target_component = compid; - wp.seq = seq; - wp.current = (_mission_result.index_current_mission == seq) ? 1 : 0; - mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT seq %u to ID %u", wp.seq, wp.target_system); } - - } else { - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD"); - - if (_verbose) { warnx("WPM: Send WAYPOINT ERROR: could not read seq %u from dataman ID %i", seq, _wpm->dataman_id); } - } -} - -void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < _wpm->max_size) { - mavlink_message_t msg; - mavlink_mission_request_t wpr; - wpr.target_system = sysid; - wpr.target_component = compid; - wpr.seq = seq; - mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr); - mavlink_missionlib_send_message(&msg); - _wpm->timestamp_last_send_request = hrt_absolute_time(); - - if (_verbose) { warnx("WPM: Send WAYPOINT_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } - - } else { - mavlink_missionlib_send_gcs_string("ERROR: Waypoint index exceeds list capacity"); - - if (_verbose) { warnx("WPM: Send WAYPINT_REQUEST ERROR: seq %u exceeds list capacity", seq); } - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void Mavlink::mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_mission_item_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wp_reached); - mavlink_missionlib_send_message(&msg); - - if (_verbose) { warnx("WPM: Send WAYPOINT_REACHED seq %u", wp_reached.seq); } -} - -void Mavlink::mavlink_waypoint_eventloop(uint64_t now) -{ - /* check for timed-out operations */ - if (now - _wpm->timestamp_lastaction > _wpm->action_timeout && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) { - - mavlink_missionlib_send_gcs_string("Operation timeout"); - - if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_state); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_partner_sysid = 0; - _wpm->current_partner_compid = 0; - - } else if (now - _wpm->timestamp_last_send_request > _wpm->retry_timeout && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* try to get WP again after short timeout */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } -} - - -void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg) -{ - uint64_t now = hrt_absolute_time(); - - switch (msg->msgid) { - - case MAVLINK_MSG_ID_MISSION_ACK: { - mavlink_mission_ack_t wpa; - mavlink_msg_mission_ack_decode(msg, &wpa); - - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { - if ((msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid)) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (_wpm->current_wp_id == _wpm->size - 1) { - if (_verbose) { warnx("WPM: MISSION_ACK all items sent, switch to MAVLINK_WPM_STATE_IDLE"); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - _wpm->current_wp_id = 0; - } - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { - mavlink_mission_set_current_t wpc; - mavlink_msg_mission_set_current_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - if (wpc.seq < _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT OK (%d)", wpc.seq); } - - _mission.current_index = wpc.seq; - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: not in list (%d)", wpc.seq); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); - } - - } else { - if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { - mavlink_mission_request_list_t wprl; - mavlink_msg_mission_request_list_decode(msg, &wprl); - - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE || _wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (_wpm->size > 0) { - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST nothing to send, mission is empty"); } - } - - _wpm->current_count = _wpm->size; - mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, _wpm->current_count); - - } else { - if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } - - mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy"); - } - } - - break; - } - - case MAVLINK_MSG_ID_MISSION_REQUEST: { - mavlink_mission_request_t wpr; - mavlink_msg_mission_request_decode(msg, &wpr); - - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { - if (msg->sysid == _wpm->current_partner_sysid && msg->compid == _wpm->current_partner_compid) { - _wpm->timestamp_lastaction = now; - - if (wpr.seq >= _wpm->size) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: ignored, requested seq %u was out of bounds [0 %d]", wpr.seq, _wpm->size - 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); - break; - } - - /* - * Ensure that we are in the correct state and that the first request has id 0 - * and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq == 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: first id != 0 (%d)", wpr.seq); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq == _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u (again) from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else if (wpr.seq == _wpm->current_wp_id + 1) { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u, staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS", wpr.seq, msg->sysid); } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, should be %i or %i", wpr.seq, msg->sysid, _wpm->current_wp_id, _wpm->current_wp_id + 1); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - _wpm->current_wp_id = wpr.seq; - _wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - - if (wpr.seq < _wpm->size) { - mavlink_wpm_send_waypoint(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - - } else { - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bounds", wpr.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("REJ. WP CMD: partner id mismatch"); - - if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_COUNT: { - mavlink_mission_count_t wpc; - mavlink_msg_mission_count_decode(msg, &wpc); - - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { - _wpm->timestamp_lastaction = now; - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - - if (wpc.count > NUM_MISSIONS_SUPPORTED) { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, NUM_MISSIONS_SUPPORTED); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_NO_SPACE); - break; - } - - if (wpc.count == 0) { - if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } - - mavlink_missionlib_send_gcs_string("WP COUNT 0: CLEAR MISSION"); - break; - } - - if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - - if (_wpm->current_wp_id == 0) { - /* looks like our WAYPOINT_REQUEST was lost, try again */ - if (_verbose) { warnx("WPM: MISSION_COUNT %u (again) from ID %u", wpc.count, msg->sysid); } - - mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _wpm->current_wp_id); } - - mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); - break; - } - - } else { - if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _wpm->current_state); } - - mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy"); - break; - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST; - _wpm->current_wp_id = 0; - _wpm->current_partner_sysid = msg->sysid; - _wpm->current_partner_compid = msg->compid; - _wpm->current_count = wpc.count; - _wpm->current_dataman_id = _wpm->dataman_id == 0 ? 1 : 0; // use inactive storage for transmission - - _mission.current_index = -1; - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - - case MAVLINK_MSG_ID_MISSION_ITEM: { - mavlink_mission_item_t wp; - mavlink_msg_mission_item_decode(msg, &wp); - - if (wp.target_system == mavlink_system.sysid && wp.target_component == _mavlink_wpm_comp_id) { - _wpm->timestamp_lastaction = now; - - /* Ensure that we are in the correct state and that the first waypoint has id 0 - * and the following waypoints have the correct ids - */ - if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { - if (wp.seq != 0) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq != 0 (%d)", wp.seq); } - break; - } - - } else if (_wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (wp.seq >= _wpm->current_count) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: first item seq %d out of bounds [0 %d]", wp.seq, _wpm->current_count - 1); } - - mavlink_missionlib_send_gcs_string("Ignored MISSION_ITEM WP out of bounds"); - break; - } - - if (wp.seq != _wpm->current_wp_id) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _wpm->current_wp_id); } - - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - break; - } - } - - _wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - - struct mission_item_s mission_item; - int ret = map_mavlink_mission_item_to_mission_item(&wp, &mission_item); - - if (ret != OK) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, ret); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - dm_item_t dm_item = _wpm->current_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { - if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _wpm->current_dataman_id); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD"); - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - break; - } - - /* waypoint marked as current */ - if (wp.current) { - _mission.current_index = wp.seq; - } - - if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } - - _wpm->current_wp_id = wp.seq + 1; - - if (_wpm->current_wp_id == _wpm->current_count && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - /* got all new mission items successfully */ - if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, changing state to MAVLINK_WPM_STATE_IDLE", _wpm->current_count); } - - _wpm->current_state = MAVLINK_WPM_STATE_IDLE; - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->dataman_id = _wpm->current_dataman_id; - _wpm->size = _wpm->current_count; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = _wpm->current_count; - - publish_mission(); - - } else { - /* request next item */ - mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id); - } - } - break; - } - - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { - mavlink_mission_clear_all_t wpca; - mavlink_msg_mission_clear_all_decode(msg, &wpca); - - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { - - if (_wpm->current_state == MAVLINK_WPM_STATE_IDLE) { - /* clear only active dataman storage, don't change storage id */ - dm_item_t dm_id = _wpm->dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; - - if (dm_clear(dm_id) == OK) { - if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ACCEPTED); - - /* update WPM state */ - _wpm->timestamp_lastaction = now; - _wpm->size = 0; - - /* update mission topic */ - _mission.dataman_id = _wpm->dataman_id; - _mission.count = 0; - _mission.current_index = -1; - - publish_mission(); - - } else { - if (_verbose) { warnx("WPM: CLEAR_ALL ERROR: can't clear dataman mission storage"); } - - mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR); - } - - } else { - mavlink_missionlib_send_gcs_string("IGN WP CLEAR CMD: Busy"); - - if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } - } - } - - break; - } - - default: { - /* other messages might should get caught by mavlink and others */ - break; - } - } -} - -void -Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg) +int +Mavlink::send_statustext(const char *string) { - uint8_t buf[MAVLINK_MAX_PACKET_LEN]; - - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - mavlink_send_uart_bytes(_channel, buf, len); + return send_statustext(MAV_SEVERITY_INFO, string); } - int -Mavlink::mavlink_missionlib_send_gcs_string(const char *string) +Mavlink::send_statustext(enum MAV_SEVERITY severity, const char *string) { const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN; mavlink_statustext_t statustext; - statustext.severity = MAV_SEVERITY_INFO; + statustext.severity = severity; int i = 0; @@ -1546,7 +908,7 @@ Mavlink::mavlink_missionlib_send_gcs_string(const char *string) return OK; } else { - return 1; + return ERROR; } } @@ -1684,7 +1046,7 @@ Mavlink::message_buffer_is_empty() bool -Mavlink::message_buffer_write(void *ptr, int size) +Mavlink::message_buffer_write(const void *ptr, int size) { // bytes available to write int available = _message_buffer.read_ptr - _message_buffer.write_ptr - 1; @@ -1751,7 +1113,7 @@ Mavlink::message_buffer_mark_read(int n) } void -Mavlink::pass_message(mavlink_message_t *msg) +Mavlink::pass_message(const mavlink_message_t *msg) { if (_passing_on) { /* size is 8 bytes plus variable payload */ @@ -1762,6 +1124,11 @@ Mavlink::pass_message(mavlink_message_t *msg) } } +float +Mavlink::get_rate_mult() +{ + return _datarate / 1000.0f; +} int Mavlink::task_main(int argc, char *argv[]) @@ -1879,8 +1246,6 @@ Mavlink::task_main(int argc, char *argv[]) break; } - _mavlink_wpm_comp_id = MAV_COMP_ID_MISSIONPLANNER; - warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate); /* flush stdout in case MAVLink is about to take it over */ @@ -1922,11 +1287,11 @@ Mavlink::task_main(int argc, char *argv[]) /* start the MAVLink receiver */ _receive_thread = MavlinkReceiver::receive_start(this); - /* initialize waypoint manager */ - mavlink_wpm_init(_wpm); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); - memset(&_mission_result, 0, sizeof(_mission_result)); + /* create mission manager */ + _mission_manager = new MavlinkMissionManager(this); + _mission_manager->set_verbose(_verbose); _task_running = true; @@ -1941,7 +1306,7 @@ Mavlink::task_main(int argc, char *argv[]) MavlinkCommandsStream commands_stream(this, _channel); /* add default streams depending on mode and intervals depending on datarate */ - float rate_mult = _datarate / 1000.0f; + float rate_mult = get_rate_mult(); configure_stream("HEARTBEAT", 1.0f); @@ -1976,7 +1341,6 @@ Mavlink::task_main(int argc, char *argv[]) /* don't send parameters on startup without request */ _mavlink_param_queue_index = param_count(); - MavlinkRateLimiter slow_rate_limiter(2000000.0f / rate_mult); MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult); /* set main loop delay depending on data rate to minimize CPU overhead */ @@ -2030,36 +1394,16 @@ Mavlink::task_main(int argc, char *argv[]) stream->update(t); } - bool updated; - orb_check(mission_result_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(mission_result), mission_result_sub, &_mission_result); - - if (_verbose) { warnx("WPM: got mission result, new current: %d", _mission_result.index_current_mission); } - - if (_mission_result.mission_reached) { - mavlink_wpm_send_waypoint_reached((uint16_t)_mission_result.mission_index_reached); - } - - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - - } else { - if (slow_rate_limiter.check(t)) { - mavlink_wpm_send_waypoint_current((uint16_t)_mission_result.index_current_mission); - } - } - if (fast_rate_limiter.check(t)) { mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(hrt_absolute_time()); + _mission_manager->eventloop(); if (!mavlink_logbuffer_is_empty(&_logbuffer)) { struct mavlink_logmessage msg; int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg); if (lb_ret == OK) { - mavlink_missionlib_send_gcs_string(msg.text); + send_statustext(msg.text); } } } @@ -2093,11 +1437,11 @@ Mavlink::task_main(int argc, char *argv[]) } } - - perf_end(_loop_perf); } + delete _mission_manager; + delete _subscribe_to_stream; _subscribe_to_stream = nullptr; diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 5d7273abd..085a97d73 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -56,51 +56,7 @@ #include "mavlink_orb_subscription.h" #include "mavlink_stream.h" #include "mavlink_messages.h" - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds -#define MAVLINK_WPM_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; ///< Count of waypoints in active mission - uint16_t max_size; ///< Maximal count of waypoints - int dataman_id; ///< Dataman storage ID for active mission - enum MAVLINK_WPM_STATES current_state; ///< Current waypoint manager state - int16_t current_wp_id; ///< Waypoint ID in current transmission - uint16_t current_count; ///< Waypoints count in current transmission - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - int current_dataman_id; ///< Dataman storage ID for current transmission - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_last_send_request; - uint32_t action_timeout; - uint32_t retry_timeout; -}; +#include "mavlink_mission.h" class Mavlink @@ -143,7 +99,7 @@ public: static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(const mavlink_message_t *msg, Mavlink *self); static int get_uart_fd(unsigned index); @@ -168,10 +124,6 @@ public: bool get_forwarding_on() { return _forwarding_on; } - /** - * Handle waypoint related messages. - */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); static int start_helper(int argc, char *argv[]); @@ -192,6 +144,10 @@ public: */ int set_hil_enabled(bool hil_enabled); + void send_message(const mavlink_message_t *msg); + + void handle_message(const mavlink_message_t *msg); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); int get_instance_id(); @@ -209,6 +165,10 @@ public: int get_mavlink_fd() { return _mavlink_fd; } + int send_statustext(const char *string); + int send_statustext(enum MAV_SEVERITY severity, const char *string); + + float get_rate_mult(); /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } @@ -238,12 +198,12 @@ private: MavlinkOrbSubscription *_subscriptions; MavlinkStream *_streams; + MavlinkMissionManager *_mission_manager; + orb_advert_t _mission_pub; - struct mission_s _mission; - struct mission_result_s _mission_result; + int _mission_result_sub; MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; @@ -251,10 +211,6 @@ private: pthread_t _receive_thread; - /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; - bool _verbose; bool _forwarding_on; bool _passing_on; @@ -336,20 +292,9 @@ private: void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); - void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); - void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); - void mavlink_wpm_send_waypoint_current(uint16_t seq); - void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type); - void mavlink_wpm_init(mavlink_wpm_storage *state); - int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); - void publish_mission(); - - void mavlink_missionlib_send_message(mavlink_message_t *msg); - int mavlink_missionlib_send_gcs_string(const char *string); + uint8_t get_system_id(); + + uint8_t get_component_id(); int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); @@ -364,13 +309,13 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); - void pass_message(mavlink_message_t *msg); + void pass_message(const mavlink_message_t *msg); static int mavlink_dev_ioctl(struct file *filep, int cmd, unsigned long arg); @@ -378,5 +323,4 @@ private: * Main mavlink task. */ int task_main(int argc, char *argv[]); - }; diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp new file mode 100644 index 000000000..1eab7d761 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -0,0 +1,823 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 mavlink_mission.cpp + * MAVLink mission manager implementation. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#include "mavlink_mission.h" +#include "mavlink_main.h" + +#include +#include +#include +#include + +#include +#include +#include + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + + +MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : + _mavlink(mavlink), + _channel(mavlink->get_channel()), + _comp_id(MAV_COMP_ID_MISSIONPLANNER), + _state(MAVLINK_WPM_STATE_IDLE), + _time_last_recv(0), + _time_last_sent(0), + _action_timeout(MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT), + _retry_timeout(MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT), + _max_count(DM_KEY_WAYPOINTS_OFFBOARD_0_MAX), + _count(0), + _current_seq(0), + _dataman_id(0), + _transfer_dataman_id(0), + _transfer_count(0), + _transfer_seq(0), + _transfer_current_seq(0), + _transfer_partner_sysid(0), + _transfer_partner_compid(0), + _offboard_mission_sub(-1), + _mission_result_sub(-1), + _offboard_mission_pub(-1), + _slow_rate_limiter(2000000.0f / mavlink->get_rate_mult()), + _verbose(false) +{ + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + + init_offboard_mission(); +} + +MavlinkMissionManager::~MavlinkMissionManager() +{ + close(_offboard_mission_pub); + close(_mission_result_sub); +} + +void +MavlinkMissionManager::init_offboard_mission() +{ + mission_s mission_state; + if (dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)) == sizeof(mission_s)) { + _dataman_id = mission_state.dataman_id; + _count = mission_state.count; + _current_seq = mission_state.current_seq; + + warnx("offboard mission init: dataman_id=%d, count=%u, current_seq=%d", _dataman_id, _count, _current_seq); + + } else { + _dataman_id = 0; + _count = 0; + _current_seq = 0; + warnx("offboard mission init: ERROR, reading mission state failed"); + } +} + +/** + * Write new mission state to dataman and publish offboard_mission topic to notify navigator about changes. + */ +int +MavlinkMissionManager::update_active_mission(int dataman_id, unsigned count, int seq) +{ + struct mission_s mission; + + mission.dataman_id = dataman_id; + mission.count = count; + mission.current_seq = seq; + + /* update mission state in dataman */ + /* lock MISSION_STATE item */ + dm_lock(DM_KEY_MISSION_STATE); + + int res = dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission, sizeof(mission_s)); + + /* unlock MISSION_STATE item */ + dm_unlock(DM_KEY_MISSION_STATE); + + if (res == sizeof(mission_s)) { + /* update active mission state */ + _dataman_id = dataman_id; + _count = count; + _current_seq = seq; + + /* mission state saved successfully, publish offboard_mission topic */ + if (_offboard_mission_pub < 0) { + _offboard_mission_pub = orb_advertise(ORB_ID(offboard_mission), &mission); + + } else { + orb_publish(ORB_ID(offboard_mission), _offboard_mission_pub, &mission); + } + + return OK; + + } else { + warnx("ERROR: can't save mission state"); + _mavlink->send_statustext(MAV_SEVERITY_CRITICAL, "ERROR: can't save mission state"); + + return ERROR; + } +} + +void +MavlinkMissionManager::send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_mission_ack_t wpa; + + wpa.target_system = sysid; + wpa.target_component = compid; + wpa.type = type; + + mavlink_msg_mission_ack_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpa); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ACK type %u to ID %u", wpa.type, wpa.target_system); } +} + + +void +MavlinkMissionManager::send_mission_current(uint16_t seq) +{ + if (seq < _count) { + mavlink_message_t msg; + mavlink_mission_current_t wpc; + + wpc.seq = seq; + + mavlink_msg_mission_current_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + } else if (seq == 0 && _count == 0) { + /* don't broadcast if no WPs */ + + } else { + if (_verbose) { warnx("WPM: Send MISSION_CURRENT ERROR: seq %u out of bounds", seq); } + + _mavlink->send_statustext("ERROR: wp index out of bounds"); + } +} + + +void +MavlinkMissionManager::send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_count_t wpc; + + wpc.target_system = sysid; + wpc.target_component = compid; + wpc.count = _count; + + mavlink_msg_mission_count_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpc); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_COUNT %u to ID %u", wpc.count, wpc.target_system); } +} + + +void +MavlinkMissionManager::send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + dm_item_t dm_item = _dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + struct mission_item_s mission_item; + + if (dm_read(dm_item, seq, &mission_item, sizeof(struct mission_item_s)) == sizeof(struct mission_item_s)) { + _time_last_sent = hrt_absolute_time(); + + /* create mission_item_s from mavlink_mission_item_t */ + mavlink_mission_item_t wp; + format_mavlink_mission_item(&mission_item, &wp); + + mavlink_message_t msg; + wp.target_system = sysid; + wp.target_component = compid; + wp.seq = seq; + wp.current = (_current_seq == seq) ? 1 : 0; + mavlink_msg_mission_item_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM seq %u to ID %u", wp.seq, wp.target_system); } + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to read from micro SD"); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM ERROR: could not read seq %u from dataman ID %i", seq, _dataman_id); } + } +} + + +void +MavlinkMissionManager::send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < _max_count) { + _time_last_sent = hrt_absolute_time(); + + mavlink_message_t msg; + mavlink_mission_request_t wpr; + wpr.target_system = sysid; + wpr.target_component = compid; + wpr.seq = seq; + mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wpr); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST seq %u to ID %u", wpr.seq, wpr.target_system); } + + } else { + _mavlink->send_statustext("ERROR: Waypoint index exceeds list capacity"); + + if (_verbose) { warnx("WPM: Send MISSION_REQUEST ERROR: seq %u exceeds list capacity", seq); } + } +} + + +void +MavlinkMissionManager::send_mission_item_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_mission_item_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_mission_item_reached_encode_chan(mavlink_system.sysid, _comp_id, _channel, &msg, &wp_reached); + _mavlink->send_message(&msg); + + if (_verbose) { warnx("WPM: Send MISSION_ITEM_REACHED reached_seq %u", wp_reached.seq); } +} + + +void +MavlinkMissionManager::eventloop() +{ + hrt_abstime now = hrt_absolute_time(); + + bool updated = false; + orb_check(_mission_result_sub, &updated); + + if (updated) { + mission_result_s mission_result; + orb_copy(ORB_ID(mission_result), _mission_result_sub, &mission_result); + + _current_seq = mission_result.seq_current; + + if (_verbose) { warnx("WPM: got mission result, new current_seq: %d", _current_seq); } + + if (mission_result.reached) { + send_mission_item_reached((uint16_t)mission_result.seq_reached); + } + + send_mission_current(_current_seq); + + } else { + if (_slow_rate_limiter.check(now)) { + send_mission_current(_current_seq); + } + } + + /* check for timed-out operations */ + if (_state != MAVLINK_WPM_STATE_IDLE && hrt_elapsed_time(&_time_last_recv) > _action_timeout) { + _mavlink->send_statustext("Operation timeout"); + + if (_verbose) { warnx("WPM: Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE", _state); } + + _state = MAVLINK_WPM_STATE_IDLE; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + /* try to request item again after timeout */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + + } else if (_state == MAVLINK_WPM_STATE_SENDLIST && hrt_elapsed_time(&_time_last_sent) > _retry_timeout) { + if (_transfer_seq == 0) { + /* try to send items count again after timeout */ + send_mission_count(_transfer_partner_sysid, _transfer_partner_compid, _transfer_count); + + } else { + /* try to send item again after timeout */ + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq - 1); + } + } +} + + +void +MavlinkMissionManager::handle_message(const mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_MISSION_ACK: + handle_mission_ack(msg); + break; + + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: + handle_mission_set_current(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: + handle_mission_request_list(msg); + break; + + case MAVLINK_MSG_ID_MISSION_REQUEST: + handle_mission_request(msg); + break; + + case MAVLINK_MSG_ID_MISSION_COUNT: + handle_mission_count(msg); + break; + + case MAVLINK_MSG_ID_MISSION_ITEM: + handle_mission_item(msg); + break; + + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: + handle_mission_clear_all(msg); + break; + + default: + break; + } +} + + +void +MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) +{ + mavlink_mission_ack_t wpa; + mavlink_msg_mission_ack_decode(msg, &wpa); + + if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == _count) { + if (_verbose) { warnx("WPM: MISSION_ACK OK all items sent, switch to state IDLE"); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: not all items sent, switch to state IDLE anyway"); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) +{ + mavlink_mission_set_current_t wpc; + mavlink_msg_mission_set_current_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.seq < _count) { + if (update_active_mission(_dataman_id, _count, wpc.seq) == OK) { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d OK", wpc.seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR", wpc.seq); } + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT seq=%d ERROR: not in list", wpc.seq); } + + _mavlink->send_statustext("IGN WP CURR CMD: Not in list"); + } + + } else { + if (_verbose) { warnx("WPM: MISSION_SET_CURRENT ERROR: busy"); } + + _mavlink->send_statustext("IGN WP CURR CMD: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) +{ + mavlink_mission_request_list_t wprl; + mavlink_msg_mission_request_list_decode(msg, &wprl); + + if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_count > 0) { + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK nothing to send, mission is empty"); } + } + + send_mission_count(msg->sysid, msg->compid, _count); + + } else { + if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST ERROR: busy"); } + + _mavlink->send_statustext("IGN REQUEST LIST: Busy"); + } + } +} + + +void +MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) +{ + mavlink_mission_request_t wpr; + mavlink_msg_mission_request_decode(msg, &wpr); + + if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (_state == MAVLINK_WPM_STATE_SENDLIST) { + _time_last_recv = hrt_absolute_time(); + + /* _transfer_seq contains sequence of expected request */ + if (wpr.seq == _transfer_seq && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u", wpr.seq, msg->sysid); } + + _transfer_seq++; + + } else if (wpr.seq == _transfer_seq - 1 && wpr.seq >= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST seq %u from ID %u (again)", wpr.seq, msg->sysid); } + + } else { + if (_transfer_seq > 0 && _transfer_seq < _transfer_count) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i or %i", wpr.seq, msg->sysid, _transfer_seq - 1, _transfer_seq); } + + } else if (_transfer_seq <= 0) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq); } + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u from ID %u unexpected, must be %i", wpr.seq, msg->sysid, _transfer_seq - 1); } + } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + return; + } + + /* double check bounds in case of items count changed */ + if (wpr.seq >= 0 && wpr.seq < _count) { + send_mission_item(_transfer_partner_sysid, _transfer_partner_compid, wpr.seq); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: seq %u out of bound [0, %u]", wpr.seq, msg->sysid, wpr.seq, _count - 1); } + + _state = MAVLINK_WPM_STATE_IDLE; + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("REJ. WP CMD: Req. WP was unexpected"); + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM_REQUEST: No transfer"); + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: busy (state %d).", _state); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + } + + } else { + _mavlink->send_statustext("REJ. WP CMD: partner id mismatch"); + + if (_verbose) { warnx("WPM: MISSION_ITEM_REQUEST ERROR: rejected, partner ID mismatch"); } + } + } +} + + +void +MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) +{ + mavlink_mission_count_t wpc; + mavlink_msg_mission_count_decode(msg, &wpc); + + if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (_state == MAVLINK_WPM_STATE_IDLE) { + _time_last_recv = hrt_absolute_time(); + + if (wpc.count > _max_count) { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: too many waypoints (%d), supported: %d", wpc.count, _max_count); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_NO_SPACE); + return; + } + + if (wpc.count == 0) { + if (_verbose) { warnx("WPM: MISSION_COUNT 0, clearing waypoints list and staying in state MAVLINK_WPM_STATE_IDLE"); } + + /* alternate dataman ID anyway to let navigator know about changes */ + update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); + _mavlink->send_statustext("WP COUNT 0: CLEAR MISSION"); + + // TODO send ACK? + return; + } + + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u, changing state to MAVLINK_WPM_STATE_GETLIST", wpc.count, msg->sysid); } + + _state = MAVLINK_WPM_STATE_GETLIST; + _transfer_seq = 0; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + _transfer_count = wpc.count; + _transfer_dataman_id = _dataman_id == 0 ? 1 : 0; // use inactive storage for transmission + _transfer_current_seq = -1; + + } else if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (_transfer_seq == 0) { + /* looks like our MISSION_REQUEST was lost, try again */ + if (_verbose) { warnx("WPM: MISSION_COUNT %u from ID %u (again)", wpc.count, msg->sysid); } + + _mavlink->send_statustext("WP CMD OK AGAIN"); + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, already receiving seq %u", _transfer_seq); } + + _mavlink->send_statustext("REJ. WP CMD: Busy"); + return; + } + + } else { + if (_verbose) { warnx("WPM: MISSION_COUNT ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_COUNT: Busy"); + return; + } + + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } +} + + +void +MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) +{ + mavlink_mission_item_t wp; + mavlink_msg_mission_item_decode(msg, &wp); + + if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (_state == MAVLINK_WPM_STATE_GETLIST) { + _time_last_recv = hrt_absolute_time(); + + if (wp.seq != _transfer_seq) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } + + /* don't send request here, it will be performed in eventloop after timeout */ + return; + } + + } else if (_state == MAVLINK_WPM_STATE_IDLE) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } + + _mavlink->send_statustext("IGN MISSION_ITEM: No transfer"); + return; + + } else { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } + + _mavlink->send_statustext("IGN MISSION_ITEM: Busy"); + return; + } + + struct mission_item_s mission_item; + int ret = parse_mavlink_mission_item(&wp, &mission_item); + + if (ret != OK) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + dm_item_t dm_item = _transfer_dataman_id == 0 ? DM_KEY_WAYPOINTS_OFFBOARD_0 : DM_KEY_WAYPOINTS_OFFBOARD_1; + + if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { + if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } + + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + _mavlink->send_statustext("#audio: Unable to write on micro SD"); + _state = MAVLINK_WPM_STATE_IDLE; + return; + } + + /* waypoint marked as current */ + if (wp.current) { + _transfer_current_seq = wp.seq; + } + + if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } + + _transfer_seq = wp.seq + 1; + + if (_transfer_seq == _transfer_count) { + /* got all new mission items successfully */ + if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } + + _state = MAVLINK_WPM_STATE_IDLE; + + if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + /* request next item */ + send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); + } + } +} + + +void +MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) +{ + mavlink_mission_clear_all_t wpca; + mavlink_msg_mission_clear_all_decode(msg, &wpca); + + if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + + if (_state == MAVLINK_WPM_STATE_IDLE) { + /* don't touch mission items storage itself, but only items count in mission state */ + _time_last_recv = hrt_absolute_time(); + + if (update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0) == OK) { + if (_verbose) { warnx("WPM: CLEAR_ALL OK"); } + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); + + } else { + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); + } + + } else { + _mavlink->send_statustext("IGN WP CLEAR CMD: Busy"); + + if (_verbose) { warnx("WPM: CLEAR_ALL IGNORED: busy"); } + } + } +} + +int +MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item) +{ + /* only support global waypoints for now */ + switch (mavlink_mission_item->frame) { + case MAV_FRAME_GLOBAL: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = false; + break; + + case MAV_FRAME_GLOBAL_RELATIVE_ALT: + mission_item->lat = (double)mavlink_mission_item->x; + mission_item->lon = (double)mavlink_mission_item->y; + mission_item->altitude = mavlink_mission_item->z; + mission_item->altitude_is_relative = true; + break; + + case MAV_FRAME_LOCAL_NED: + case MAV_FRAME_LOCAL_ENU: + return MAV_MISSION_UNSUPPORTED_FRAME; + + case MAV_FRAME_MISSION: + default: + return MAV_MISSION_ERROR; + } + + switch (mavlink_mission_item->command) { + case MAV_CMD_NAV_TAKEOFF: + mission_item->pitch_min = mavlink_mission_item->param1; + break; + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; + default: + mission_item->acceptance_radius = mavlink_mission_item->param2; + mission_item->time_inside = mavlink_mission_item->param1; + break; + } + + mission_item->yaw = _wrap_pi(mavlink_mission_item->param4 * M_DEG_TO_RAD_F); + mission_item->loiter_radius = fabsf(mavlink_mission_item->param3); + mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */ + mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command; + + mission_item->autocontinue = mavlink_mission_item->autocontinue; + // mission_item->index = mavlink_mission_item->seq; + mission_item->origin = ORIGIN_MAVLINK; + + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + + return MAV_MISSION_ACCEPTED; +} + + +int +MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item) +{ + if (mission_item->altitude_is_relative) { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL; + + } else { + mavlink_mission_item->frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; + } + + switch (mission_item->nav_cmd) { + case NAV_CMD_TAKEOFF: + mavlink_mission_item->param1 = mission_item->pitch_min; + break; + + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + + default: + mavlink_mission_item->param2 = mission_item->acceptance_radius; + mavlink_mission_item->param1 = mission_item->time_inside; + break; + } + + mavlink_mission_item->x = (float)mission_item->lat; + mavlink_mission_item->y = (float)mission_item->lon; + mavlink_mission_item->z = mission_item->altitude; + + mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F; + mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction; + mavlink_mission_item->command = mission_item->nav_cmd; + mavlink_mission_item->autocontinue = mission_item->autocontinue; + // mavlink_mission_item->seq = mission_item->index; + + return OK; +} diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h new file mode 100644 index 000000000..d8695cf83 --- /dev/null +++ b/src/modules/mavlink/mavlink_mission.h @@ -0,0 +1,176 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 mavlink_mission.h + * MAVLink mission manager interface definition. + * + * @author Lorenz Meier + * @author Julian Oes + * @author Anton Babushkin + */ + +#pragma once + +#include "mavlink_bridge_header.h" +#include "mavlink_rate_limiter.h" +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES { + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES { + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + +#define MAVLINK_MISSION_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication action timeout in useconds +#define MAVLINK_MISSION_RETRY_TIMEOUT_DEFAULT 500000 ///< Protocol communication retry timeout in useconds + +class Mavlink; + +class MavlinkMissionManager { +public: + MavlinkMissionManager(Mavlink *parent); + + ~MavlinkMissionManager(); + + void init_offboard_mission(); + + int update_active_mission(int dataman_id, unsigned count, int seq); + + void send_message(mavlink_message_t *msg); + + /** + * @brief Sends an waypoint ack message + */ + void send_mission_ack(uint8_t sysid, uint8_t compid, uint8_t type); + + /** + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ + void send_mission_current(uint16_t seq); + + void send_mission_count(uint8_t sysid, uint8_t compid, uint16_t count); + + void send_mission_item(uint8_t sysid, uint8_t compid, uint16_t seq); + + void send_mission_request(uint8_t sysid, uint8_t compid, uint16_t seq); + + /** + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ + void send_mission_item_reached(uint16_t seq); + + void eventloop(); + + void handle_message(const mavlink_message_t *msg); + + void handle_mission_ack(const mavlink_message_t *msg); + + void handle_mission_set_current(const mavlink_message_t *msg); + + void handle_mission_request_list(const mavlink_message_t *msg); + + void handle_mission_request(const mavlink_message_t *msg); + + void handle_mission_count(const mavlink_message_t *msg); + + void handle_mission_item(const mavlink_message_t *msg); + + void handle_mission_clear_all(const mavlink_message_t *msg); + + /** + * Parse mavlink MISSION_ITEM message to get mission_item_s. + */ + int parse_mavlink_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); + + /** + * Format mission_item_s as mavlink MISSION_ITEM message. + */ + int format_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); + + void set_verbose(bool v) { _verbose = v; } + +private: + Mavlink* _mavlink; + mavlink_channel_t _channel; + uint8_t _comp_id; + + enum MAVLINK_WPM_STATES _state; ///< Current state + + uint64_t _time_last_recv; + uint64_t _time_last_sent; + + uint32_t _action_timeout; + uint32_t _retry_timeout; + unsigned _max_count; ///< Maximal count of mission items + + int _dataman_id; ///< Dataman storage ID for active mission + unsigned _count; ///< Count of items in active mission + int _current_seq; ///< Current item sequence in active mission + + int _transfer_dataman_id; ///< Dataman storage ID for current transmission + unsigned _transfer_count; ///< Items count in current transmission + int _transfer_seq; ///< Item sequence in current transmission + int _transfer_current_seq; ///< Current item ID for current transmission (-1 means not initialized) + uint8_t _transfer_partner_sysid; ///< Partener SysID for current transmission + uint8_t _transfer_partner_compid; ///< Partner ComponentID for current transmission + + int _offboard_mission_sub; + int _mission_result_sub; + orb_advert_t _offboard_mission_pub; + + MavlinkRateLimiter _slow_rate_limiter; + + bool _verbose; +}; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1e217ec70..834852d6f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -79,7 +79,6 @@ __BEGIN_DECLS #include "mavlink_bridge_header.h" #include "mavlink_receiver.h" #include "mavlink_main.h" -#include "util.h" __END_DECLS @@ -902,16 +901,8 @@ MavlinkReceiver::receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* handle packet with waypoint component */ - _mavlink->mavlink_wpm_message_handler(&msg); - - /* handle packet with parameter component */ - _mavlink->mavlink_pm_message_handler(_mavlink->get_channel(), &msg); - - if (_mavlink->get_forwarding_on()) { - /* forward any messages to other mavlink instances */ - Mavlink::forward_message(&msg, _mavlink); - } + /* handle packet with parent object */ + _mavlink->handle_message(&msg); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..df5d037f8 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -100,8 +100,6 @@ public: static void *start_helper(void *context); private: - perf_counter_t _loop_perf; /**< loop performance counter */ - Mavlink *_mavlink; void handle_message(mavlink_message_t *msg); diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f532e26fe..725a9e184 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,6 +39,7 @@ MODULE_COMMAND = mavlink SRCS += mavlink_main.cpp \ mavlink.c \ mavlink_receiver.cpp \ + mavlink_mission.cpp \ mavlink_orb_subscription.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h deleted file mode 100644 index 5ca9a085d..000000000 --- a/src/modules/mavlink/util.h +++ /dev/null @@ -1,53 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 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 util.h - * Utility and helper functions and data. - */ - -#pragma once - -/** MAVLink communications channel */ -extern uint8_t chan; - -/** Shutdown marker */ -extern volatile bool thread_should_exit; - -/** Waypoint storage */ -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_base_mode, uint32_t *mavlink_custom_mode); diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h deleted file mode 100644 index a2109f2d8..000000000 --- a/src/modules/mavlink/waypoints.h +++ /dev/null @@ -1,111 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2009-2014 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 waypoints.h - * MAVLink waypoint protocol definition (BSD-relicensed). - * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - */ - -#ifndef WAYPOINTS_H_ -#define WAYPOINTS_H_ - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ - -#include -#include "mavlink_bridge_header.h" -#include -#include - -enum MAVLINK_WPM_STATES { - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES { - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -#define MAVLINK_WPM_MAX_WP_COUNT 255 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 5000000 ///< Protocol communication timeout in useconds -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 ///< When to send a new setpoint -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40000 - - -struct mavlink_wpm_storage { - uint16_t size; - uint16_t max_size; - enum MAVLINK_WPM_STATES current_state; - int16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint32_t action_timeout; - int current_dataman_id; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -int map_mavlink_mission_item_to_mission_item(const mavlink_mission_item_t *mavlink_mission_item, struct mission_item_s *mission_item); -int map_mission_item_to_mavlink_mission_item(const struct mission_item_s *mission_item, mavlink_mission_item_t *mavlink_mission_item); - - -void mavlink_wpm_init(mavlink_wpm_storage *state); -void mavlink_waypoint_eventloop(uint64_t now); -void mavlink_wpm_message_handler(const mavlink_message_t *msg); - -extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, - float param2, float param3, float param4, float param5_lat_x, - float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command); - -static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN]; - -#endif /* WAYPOINTS_H_ */ diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index d9d8353f6..f6d304c37 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -67,13 +67,11 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _mission_result_pub(-1), _mission_result({0}), - _mission_type(MISSION_TYPE_NONE) + _mission_type(MISSION_TYPE_NONE), + _inited(false) { /* load initial params */ updateParams(); - /* set initial mission items */ - on_inactive(); - } Mission::~Mission() @@ -85,16 +83,25 @@ Mission::on_inactive() { _first_run = true; - /* check anyway if missions have changed so that feedback to groundstation is given */ - bool onboard_updated; - orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); - if (onboard_updated) { - update_onboard_mission(); - } + if (_inited) { + /* check if missions have changed so that feedback to ground station is given */ + bool onboard_updated = false; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } - bool offboard_updated; - orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); - if (offboard_updated) { + bool offboard_updated = false; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + } else { + /* read mission topics on initialization */ + _inited = true; + + update_onboard_mission(); update_offboard_mission(); } } @@ -105,13 +112,13 @@ Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool updated = false; /* check if anything has changed */ - bool onboard_updated; + bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); if (onboard_updated) { update_onboard_mission(); } - bool offboard_updated; + bool offboard_updated = false; orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); if (offboard_updated) { update_offboard_mission(); @@ -139,9 +146,9 @@ Mission::update_onboard_mission() { if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { /* accept the current index set by the onboard mission if it is within bounds */ - if (_onboard_mission.current_index >=0 - && _onboard_mission.current_index < (int)_onboard_mission.count) { - _current_onboard_mission_index = _onboard_mission.current_index; + if (_onboard_mission.current_seq >=0 + && _onboard_mission.current_seq < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_seq; } else { /* if less WPs available, reset to first WP */ if (_current_onboard_mission_index >= (int)_onboard_mission.count) { @@ -154,7 +161,7 @@ Mission::update_onboard_mission() } } else { _onboard_mission.count = 0; - _onboard_mission.current_index = 0; + _onboard_mission.current_seq = 0; _current_onboard_mission_index = 0; } } @@ -163,13 +170,12 @@ void Mission::update_offboard_mission() { if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { - + warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ - if (_offboard_mission.current_index >= 0 - && _offboard_mission.current_index < (int)_offboard_mission.count) { - _current_offboard_mission_index = _offboard_mission.current_index; + if (_offboard_mission.current_seq >= 0 && _offboard_mission.current_seq < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_seq; } else { - /* if less WPs available, reset to first WP */ + /* if less items available, reset to first item */ if (_current_offboard_mission_index >= (int)_offboard_mission.count) { _current_offboard_mission_index = 0; /* if not initialized, set it to 0 */ @@ -194,10 +200,12 @@ Mission::update_offboard_mission() _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { + warnx("offboard mission update failed"); _offboard_mission.count = 0; - _offboard_mission.current_index = 0; + _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; } + report_current_offboard_mission_item(); } @@ -326,10 +334,10 @@ Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *curren void Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) { - int next_temp_mission_index = _onboard_mission.current_index + 1; + int next_temp_mission_index = _onboard_mission.current_seq + 1; /* try if there is a next onboard mission */ - if (_onboard_mission.current_index >= 0 && + if (_onboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_onboard_mission.count) { struct mission_item_s new_mission_item; if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { @@ -349,9 +357,9 @@ void Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) { /* try if there is a next offboard mission */ - int next_temp_mission_index = _offboard_mission.current_index + 1; + int next_temp_mission_index = _offboard_mission.current_seq + 1; warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); - if (_offboard_mission.current_index >= 0 && + if (_offboard_mission.current_seq >= 0 && next_temp_mission_index < (int)_offboard_mission.count) { dm_item_t dm_current; if (_offboard_mission.dataman_id == 0) { @@ -441,19 +449,31 @@ Mission::save_offboard_mission_state() /* read current state */ int read_res = dm_read(DM_KEY_MISSION_STATE, 0, &mission_state, sizeof(mission_s)); - /* check if state actually changed to save flash write cycles */ - if (read_res != sizeof(mission_s) || mission_state.dataman_id != _offboard_mission.dataman_id || - mission_state.count != _offboard_mission.count || - mission_state.current_index != _current_offboard_mission_index) { + if (read_res == sizeof(mission_s)) { + /* data read successfully, check dataman ID and items count */ + if (mission_state.dataman_id == _offboard_mission.dataman_id && mission_state.count == _offboard_mission.count) { + /* navigator may modify only sequence, write modified state only if it changed */ + if (mission_state.current_seq != _current_offboard_mission_index) { + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); + } + } + } + } else { + /* invalid data, this must not happen and indicates error in offboard_mission publisher */ mission_state.dataman_id = _offboard_mission.dataman_id; mission_state.count = _offboard_mission.count; - mission_state.current_index = _current_offboard_mission_index; + mission_state.current_seq = _current_offboard_mission_index; - /* write modifyed state only if changed */ - if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { - mavlink_log_critical(_navigator->get_mavlink_fd(), "error saving mission state"); + warnx("ERROR: invalid mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: invalid mission state"); + /* write modified state only if changed */ + if (dm_write(DM_KEY_MISSION_STATE, 0, DM_PERSIST_POWER_ON_RESET, &mission_state, sizeof(mission_s)) != sizeof(mission_s)) { + warnx("ERROR: can't save mission state"); + mavlink_log_critical(_navigator->get_mavlink_fd(), "ERROR: can't save mission state"); } } @@ -465,8 +485,8 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; } publish_mission_result(); } @@ -474,7 +494,8 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); save_offboard_mission_state(); @@ -483,7 +504,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.mission_finished = true; + _mission_result.finished = true; publish_mission_result(); } @@ -500,6 +521,6 @@ Mission::publish_mission_result() _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } /* reset reached bool */ - _mission_result.mission_reached = false; - _mission_result.mission_finished = false; + _mission_result.reached = false; + _mission_result.finished = false; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 98b11bade..1310671b0 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -183,6 +183,8 @@ private: MISSION_TYPE_OFFBOARD } _mission_type; + bool _inited; + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ }; diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 4bcaaaa5a..499b2e1e5 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -97,11 +97,15 @@ struct mission_item_s { unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; +/** + * This topic used to notify navigator about mission changes, mission itself and new mission state + * must be stored in dataman before publication. + */ struct mission_s { int dataman_id; /**< default 0, there are two offboard storage places in the dataman: 0 or 1 */ unsigned count; /**< count of the missions stored in the dataman */ - int current_index; /**< default -1, start at the one changed latest */ + int current_seq; /**< default -1, start at the one changed latest */ }; /** diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index ad654a9ff..beb797e62 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -53,10 +53,10 @@ struct mission_result_s { - bool mission_reached; /**< true if mission has been reached */ - unsigned mission_index_reached; /**< index of the mission which has been reached */ - unsigned index_current_mission; /**< index of the current mission */ - bool mission_finished; /**< true if mission has been completed */ + unsigned seq_reached; /**< Sequence of the mission item which has been reached */ + unsigned seq_current; /**< Sequence of the current mission item */ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ }; /** -- cgit v1.2.3 From 74ae18c245514d918e6ea58f05de98b9fe4b63a5 Mon Sep 17 00:00:00 2001 From: Benjamin O'Connell-Armand Date: Sat, 14 Jun 2014 18:46:18 -0400 Subject: Fixed rates flags --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2c36b2e6c..7baaf7139 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1834,7 +1834,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: - control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = false; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; -- cgit v1.2.3 From e0ed0625f81841194b4bd9b26c7e047a1f68a1fc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 16 Jun 2014 17:34:21 +0200 Subject: commander: failsafe_state removed, replaced with bool failsafe, navigation state and failsafe determined directly from main state and conditions --- src/modules/commander/commander.cpp | 90 +++++++++----------- src/modules/commander/state_machine_helper.cpp | 112 +++++++++++++++---------- src/modules/commander/state_machine_helper.h | 2 - src/modules/gpio_led/gpio_led.c | 2 +- src/modules/mavlink/mavlink_messages.cpp | 4 +- src/modules/mavlink/mavlink_receiver.h | 2 + src/modules/navigator/navigator_main.cpp | 4 +- src/modules/sdlog2/sdlog2.c | 2 +- src/modules/uORB/topics/vehicle_status.h | 25 +++--- 9 files changed, 125 insertions(+), 118 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e992706ac..43683f833 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -77,6 +77,7 @@ #include #include #include +#include #include #include @@ -122,6 +123,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 +#define DL_TIMEOUT 5 * 1000* 1000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -495,12 +497,12 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -639,13 +641,6 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[5] = "REBOOT"; arming_states_str[6] = "IN_AIR_RESTORE"; - char *failsafe_states_str[FAILSAFE_STATE_MAX]; - failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL_RC"; - failsafe_states_str[2] = "RTL_DL"; - failsafe_states_str[3] = "LAND"; - failsafe_states_str[4] = "TERMINATION"; - /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -666,10 +661,10 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAVIGATION_STATE_MANUAL; + status.nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; - status.failsafe_state = FAILSAFE_STATE_NORMAL; + status.failsafe = false; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -678,6 +673,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status.offboard_control_signal_lost = true; + status.data_link_lost = true; /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -771,6 +767,11 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to telemetry status */ + int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -908,6 +909,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + orb_check(telemetry_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -1186,7 +1193,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); } - /* start RC input check */ + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -1198,9 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); status_changed = true; - - status.failsafe_state = FAILSAFE_STATE_NORMAL; - failsafe_state_changed = true; } } @@ -1271,12 +1275,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - - // if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - // /* recover from failsafe */ - // (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - // } - /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); @@ -1295,34 +1293,24 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; - - if (!(status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && !mission_result.mission_finished)) { - - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - } else { - status.failsafe_state = FAILSAFE_STATE_LAND; - } - failsafe_state_changed = true; - } else { - mavlink_log_info(mavlink_fd, "#audio: no RTL during Mission"); - } } } - /* hack to detect if we finished a mission after we lost RC, so that we can trigger RTL now */ - if (status.rc_signal_lost && status.set_nav_state == NAVIGATION_STATE_AUTO_MISSION && - mission_result.mission_finished && status.failsafe_state != FAILSAFE_STATE_RC_LOSS) { - /* if we have a global position, we can switch to RTL, if not, we can try to land */ - if (status.condition_global_position_valid) { - status.failsafe_state = FAILSAFE_STATE_RC_LOSS; - mavlink_log_info(mavlink_fd, "#audio: RTL after Mission is finished"); - } else { - /* this probably doesn't make sense since we are in mission and have global position */ - status.failsafe_state = FAILSAFE_STATE_LAND; + /* data link check */ + if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where RC signal was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = false; + status_changed = true; } - failsafe_state_changed = true; } /* handle commands last, as the system needs to be updated to handle them */ @@ -1377,7 +1365,7 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; - /* now set nav state according to failsafe and main state */ + /* now set navigation state according to failsafe and main state */ set_nav_state(&status); if (main_state_changed) { @@ -1388,7 +1376,7 @@ int commander_thread_main(int argc, char *argv[]) if (failsafe_state_changed) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); failsafe_state_changed = false; } @@ -1415,7 +1403,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1519,7 +1507,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ @@ -1667,7 +1655,7 @@ set_control_mode() control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - switch (status.set_nav_state) { + switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -1719,8 +1707,6 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c52e618ef..df718ff99 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -354,7 +354,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, } } - if (ret = TRANSITION_CHANGED) { + if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; current_status->timestamp = hrt_absolute_time(); // XXX also set lockdown here @@ -363,67 +363,95 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, return ret; } - /** * Check failsafe and main status and set navigation status for navigator accordingly */ void set_nav_state(struct vehicle_status_s *status) { - switch (status->failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* evaluate main state to decide in normal (non-failsafe) mode */ - switch (status->main_state) { - case MAIN_STATE_MANUAL: - status->set_nav_state = NAVIGATION_STATE_MANUAL; - break; - - case MAIN_STATE_ALTCTL: - status->set_nav_state = NAVIGATION_STATE_ALTCTL; - break; + status->failsafe = false; - case MAIN_STATE_POSCTL: - status->set_nav_state = NAVIGATION_STATE_POSCTL; - break; - - case MAIN_STATE_AUTO_MISSION: - status->set_nav_state = NAVIGATION_STATE_AUTO_MISSION; - break; - - case MAIN_STATE_AUTO_LOITER: - status->set_nav_state = NAVIGATION_STATE_AUTO_LOITER; - break; + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_ACRO: + case MAIN_STATE_MANUAL: + case MAIN_STATE_ALTCTL: + case MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if (status->rc_signal_lost) { + status->failsafe = true; - case MAIN_STATE_AUTO_RTL: - status->set_nav_state = NAVIGATION_STATE_AUTO_RTL; - break; + } else { + switch (status->main_state) { + case MAIN_STATE_ACRO: + status->nav_state = NAVIGATION_STATE_ACRO; + break; + + case MAIN_STATE_MANUAL: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + + case MAIN_STATE_ALTCTL: + status->nav_state = NAVIGATION_STATE_ALTCTL; + break; + + case MAIN_STATE_POSCTL: + status->nav_state = NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + } + } + break; - case MAIN_STATE_ACRO: - status->set_nav_state = NAVIGATION_STATE_ACRO; - break; + case MAIN_STATE_AUTO_MISSION: + /* require data link and global position */ + if (status->data_link_lost || !status->condition_global_position_valid) { + status->failsafe = true; - default: - break; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; - case FAILSAFE_STATE_RC_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS; - break; + case MAIN_STATE_AUTO_LOITER: + /* require data link and local position */ + if (status->data_link_lost || !status->condition_local_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_DL_LOSS: - status->set_nav_state = NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } break; - case FAILSAFE_STATE_LAND: - status->set_nav_state = NAVIGATION_STATE_LAND; - break; + case MAIN_STATE_AUTO_RTL: + /* require global position and home */ + if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + status->failsafe = true; - case FAILSAFE_STATE_TERMINATION: - status->set_nav_state = NAVIGATION_STATE_TERMINATION; + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } break; default: break; } + + if (status->failsafe) { + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 594bf23a1..062271764 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,8 +63,6 @@ transition_result_t arming_state_transition(struct vehicle_status_s *current_sta transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); - transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); void set_nav_state(struct vehicle_status_s *status); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 6dfd22fdf..839a7890b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -264,7 +264,7 @@ void gpio_led_cycle(FAR void *arg) pattern = 0x2A; // *_*_*_ fast blink (armed, error) } else if (priv->status.arming_state == ARMING_STATE_ARMED) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) { pattern = 0x3f; // ****** solid (armed) } else { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b58fb4cb8..d7f56b63d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -118,7 +118,7 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - switch (status->set_nav_state) { + switch (status->nav_state) { case NAVIGATION_STATE_MANUAL: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED @@ -161,8 +161,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..ab3dc81c6 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -112,6 +112,7 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -138,6 +139,7 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + hrt_abstime _telemetry_heartbeat_time; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a3c190c7f..70da5393f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -336,7 +336,7 @@ Navigator::task_main() } /* Do stuff according to navigation state set by commander */ - switch (_vstatus.set_nav_state) { + switch (_vstatus.nav_state) { case NAVIGATION_STATE_MANUAL: case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: @@ -351,8 +351,6 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS: - case NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS: _navigation_mode = &_rtl; break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 53cd6a797..19872bbd0 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1117,7 +1117,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9390ff717..898c176fb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -59,7 +59,9 @@ * @addtogroup topics @{ */ -/* main state machine */ +/** + * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. + */ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, @@ -89,15 +91,9 @@ typedef enum { HIL_STATE_ON } hil_state_t; -typedef enum { - FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RC_LOSS, /**< Return To Launch on remote control loss */ - FAILSAFE_STATE_DL_LOSS, /**< Return To Launch on datalink loss */ - FAILSAFE_STATE_LAND, /**< Land as safe as possible */ - FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX, -} failsafe_state_t; - +/** + * Navigation state, i.e. "what should vehicle do". + */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ NAVIGATION_STATE_ACRO, /**< Acro mode */ @@ -106,9 +102,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_RC_LOSS, /**< Auto failsafe on RC loss mode */ - NAVIGATION_STATE_AUTO_FAILSAFE_DL_LOSS, /**< Auto failsafe on datalink loss mode */ NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ } navigation_state_t; @@ -171,10 +166,10 @@ struct vehicle_status_s { uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ main_state_t main_state; /**< main state machine */ - navigation_state_t set_nav_state; /**< set navigation state machine to specified value */ + navigation_state_t nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ - failsafe_state_t failsafe_state; /**< current failsafe state */ + bool failsafe; /**< true if system is in failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -199,6 +194,8 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ + bool data_link_lost; /**< datalink to GCS lost */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; -- cgit v1.2.3 From 61833905472465ebab615ecdd567b78eb1628d97 Mon Sep 17 00:00:00 2001 From: t0ni0 Date: Mon, 16 Jun 2014 17:47:10 -0400 Subject: Commander attitude flag fix --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7baaf7139..3da43a792 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1835,7 +1835,7 @@ set_control_mode() break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; -- cgit v1.2.3 From 55e5f747de013fb386727b41cc67bd019c129c31 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 17 Jun 2014 13:19:50 +0200 Subject: commander: modes display fixes, don't activate failsafe while disarmed --- src/modules/commander/commander.cpp | 56 +++++++++++++++++--------- src/modules/commander/state_machine_helper.cpp | 32 +++++++++++---- src/modules/commander/state_machine_helper.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 3 +- 4 files changed, 66 insertions(+), 27 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 43683f833..f8dcec8cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -624,22 +624,34 @@ int commander_thread_main(int argc, char *argv[]) warnx("starting"); char *main_states_str[MAIN_STATE_MAX]; - main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTL"; - main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO_MISSION"; - main_states_str[4] = "AUTO_LOITER"; - main_states_str[5] = "AUTO_RTL"; - main_states_str[6] = "ACRO"; + main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[0] = "INIT"; - arming_states_str[1] = "STANDBY"; - arming_states_str[2] = "ARMED"; - arming_states_str[3] = "ARMED_ERROR"; - arming_states_str[4] = "STANDBY_ERROR"; - arming_states_str[5] = "REBOOT"; - arming_states_str[6] = "IN_AIR_RESTORE"; + arming_states_str[ARMING_STATE_INIT] = "INIT"; + arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + char *nav_states_str[NAVIGATION_STATE_MAX]; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -844,7 +856,7 @@ int commander_thread_main(int argc, char *argv[]) /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; - bool failsafe_state_changed = false; + bool failsafe_old = false; while (!thread_should_exit) { @@ -1366,18 +1378,26 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ - set_nav_state(&status); + bool nav_state_changed = set_nav_state(&status); + // TODO handle mode changes by commands if (main_state_changed) { status_changed = true; + warnx("main state: %s", main_states_str[status.main_state]); mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); main_state_changed = false; } - if (failsafe_state_changed) { + if (status.failsafe != failsafe_old) { status_changed = true; mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); - failsafe_state_changed = false; + failsafe_old = status.failsafe; + } + + if (nav_state_changed) { + status_changed = true; + warnx("nav state: %s", nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index df718ff99..c0d730949 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -366,8 +366,11 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -void set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status) { + navigation_state_t nav_state_old = status->nav_state; + + bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); status->failsafe = false; /* evaluate main state to decide in normal (non-failsafe) mode */ @@ -377,7 +380,7 @@ void set_nav_state(struct vehicle_status_s *status) case MAIN_STATE_ALTCTL: case MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if (status->rc_signal_lost) { + if (status->rc_signal_lost && armed) { status->failsafe = true; } else { @@ -407,31 +410,44 @@ void set_nav_state(struct vehicle_status_s *status) case MAIN_STATE_AUTO_MISSION: /* require data link and global position */ - if (status->data_link_lost || !status->condition_global_position_valid) { + if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { status->failsafe = true; } else { - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + if (armed) { + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + + } else { + // TODO which mode should we set when disarmed? + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } } break; case MAIN_STATE_AUTO_LOITER: /* require data link and local position */ - if (status->data_link_lost || !status->condition_local_position_valid) { + if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { status->failsafe = true; } else { + // TODO which mode should we set when disarmed? status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } break; case MAIN_STATE_AUTO_RTL: /* require global position and home */ - if (!status->condition_global_position_valid || !status->condition_home_position_valid) { + if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) { status->failsafe = true; } else { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + if (armed) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + + } else { + // TODO which mode should we set when disarmed? + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } } break; @@ -453,5 +469,7 @@ void set_nav_state(struct vehicle_status_s *status) status->nav_state = NAVIGATION_STATE_TERMINATION; } } + + return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 062271764..cffbc9b8f 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -void set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 898c176fb..851938795 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -96,15 +96,16 @@ typedef enum { */ typedef enum { NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ - NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ NAVIGATION_STATE_TERMINATION, /**< Termination mode */ + NAVIGATION_STATE_MAX, } navigation_state_t; enum VEHICLE_MODE_FLAG { -- cgit v1.2.3 From e24925c743330bc3c6c0a24ba913a9c5fab5e07d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 18 Jun 2014 19:01:41 +0200 Subject: commander: added some failsafe logic --- src/modules/commander/commander.cpp | 39 +++++--- src/modules/commander/commander_params.c | 13 ++- src/modules/commander/state_machine_helper.cpp | 132 ++++++++++++++++++------- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/navigator_main.cpp | 4 + src/modules/uORB/topics/vehicle_status.h | 2 + 6 files changed, 142 insertions(+), 50 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f8dcec8cb..5954dcbb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,6 +619,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); + param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); /* welcome user */ warnx("starting"); @@ -627,9 +628,9 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; - main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; - main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; - main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -637,21 +638,23 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; - arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; - arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; char *nav_states_str[NAVIGATION_STATE_MAX]; - nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; - nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; - nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; - nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; - nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; - nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -853,6 +856,8 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; + int32_t datalink_loss_enabled = false; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -907,6 +912,7 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); + param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } orb_check(sp_man_sub, &updated); @@ -1309,8 +1315,8 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_abstime() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where RC signal was regained */ + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ if (status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: data link regained"); status.data_link_lost = false; @@ -1320,7 +1326,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = false; + status.data_link_lost = true; status_changed = true; } } @@ -1378,7 +1384,8 @@ int commander_thread_main(int argc, char *argv[]) was_armed = armed.armed; /* now set navigation state according to failsafe and main state */ - bool nav_state_changed = set_nav_state(&status); + bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, + mission_result.mission_finished); // TODO handle mode changes by commands if (main_state_changed) { @@ -1727,6 +1734,8 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 80ca68f21..4750f9d5c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -39,7 +39,7 @@ * * @author Lorenz Meier * @author Thomas Gubler - * @author Julian Oes + * @author Julian Oes */ #include @@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * Datalink loss mode enabled. + * + * Set to 1 to enable actions triggered when the datalink is lost. + * + * @group commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index c0d730949..f0fe13921 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -366,7 +366,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) { navigation_state_t nav_state_old = status->nav_state; @@ -383,6 +383,16 @@ bool set_nav_state(struct vehicle_status_s *status) if (status->rc_signal_lost && armed) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { switch (status->main_state) { case MAIN_STATE_ACRO: @@ -409,45 +419,116 @@ bool set_nav_state(struct vehicle_status_s *status) break; case MAIN_STATE_AUTO_MISSION: - /* require data link and global position */ - if ((status->data_link_lost || !status->condition_global_position_valid) && armed) { + /* go into failsafe + * - if either the datalink is enabled and lost as well as RC is lost + * - if there is no datalink and the mission is finished */ + if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + + /* don't bother if RC is lost and mission is not yet finished */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for missions */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } break; case MAIN_STATE_AUTO_LOITER: - /* require data link and local position */ - if ((status->data_link_lost || !status->condition_local_position_valid) && armed) { + /* go into failsafe if datalink and RC is lost */ + if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } else { - // TODO which mode should we set when disarmed? + /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } break; case MAIN_STATE_AUTO_RTL: /* require global position and home */ - if ((!status->condition_global_position_valid || !status->condition_home_position_valid) && armed) { + if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { status->failsafe = true; - } else { - if (armed) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - // TODO which mode should we set when disarmed? - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } break; @@ -455,21 +536,6 @@ bool set_nav_state(struct vehicle_status_s *status) break; } - if (status->failsafe) { - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - } - return status->nav_state != nav_state_old; } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index cffbc9b8f..2e076cdae 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -65,6 +65,6 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 70da5393f..50bcfd58d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,8 +351,12 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; + case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + _navigation_mode = &_rtl; /* TODO: change this to something else */ + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: default: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 851938795..16652fddf 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -101,6 +101,8 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ + NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ + NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From 62faa2ee5155253779d3b93c5a280e8918585f6e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 19 Jun 2014 10:26:56 +0200 Subject: commander/navigator: renamed FS modes to RTL and RTGS (return to ground station) --- src/modules/commander/commander.cpp | 6 ++---- src/modules/commander/px4_custom_mode.h | 1 + src/modules/commander/state_machine_helper.cpp | 12 ++++++------ src/modules/mavlink/mavlink_messages.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 3 +-- src/modules/uORB/topics/vehicle_status.h | 5 ++--- 6 files changed, 20 insertions(+), 15 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5954dcbb1..b957ad9b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -649,8 +649,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_RC_LOSS] = "AUTO_FS_RC_LOSS"; - nav_states_str[NAVIGATION_STATE_AUTO_FS_DL_LOSS] = "AUTO_FS_DL_LOSS"; + nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; @@ -1734,8 +1733,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_MISSION: case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e0f8dc95d..7f5f93801 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, + PX4_CUSTOM_SUB_MODE_AUTO_RTGS }; union px4_custom_mode { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f0fe13921..74885abf6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -384,7 +384,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -427,7 +427,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -441,7 +441,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -467,7 +467,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_RC_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -481,7 +481,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { @@ -495,7 +495,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_FS_DL_LOSS; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index d7f56b63d..1fd8b1a27 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -176,6 +176,14 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; break; + case NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; + case NAVIGATION_STATE_TERMINATION: *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 50bcfd58d..661d3d744 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -351,10 +351,9 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - case NAVIGATION_STATE_AUTO_FS_RC_LOSS: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_FS_DL_LOSS: + case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; case NAVIGATION_STATE_LAND: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 16652fddf..5dc46dacb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -100,9 +100,8 @@ typedef enum { NAVIGATION_STATE_POSCTL, /**< Position control mode */ NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ - NAVIGATION_STATE_AUTO_RTL, /**< Auto RTL mode */ - NAVIGATION_STATE_AUTO_FS_RC_LOSS, /**< Auto failsafe mode on RC loss */ - NAVIGATION_STATE_AUTO_FS_DL_LOSS, /**< Auto failsafe mode on DL loss */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From f318ee21943aa6902a7d4d0c092880a4d3c4ce8c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Jun 2014 18:15:15 +0200 Subject: Remove debug output in commander spamming console --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..df7a4c886 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,7 +1592,6 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; - warnx("NONE"); break; case SWITCH_POS_OFF: // MANUAL -- cgit v1.2.3 From 571e139c24464902470901501c5bbfb618c1258c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 21:33:03 +0200 Subject: commander: Formatting and logic flow cleanup, no functional change --- src/modules/commander/state_machine_helper.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238..1a5a52a02 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 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 @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes */ #include @@ -133,15 +134,20 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (valid_transition) { // We have a good transition. Now perform any secondary validation. if (new_arming_state == ARMING_STATE_ARMED) { - // Fail transition if we need safety switch press // Allow if coming from in air restore // Allow if HIL_STATE_ON - if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); - } + if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && + status->hil_state == HIL_STATE_OFF) { + + // Fail transition if we need safety switch press + if (safety->safety_switch_available && !safety->safety_off) { - valid_transition = false; + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); + } + + valid_transition = false; + } } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { -- cgit v1.2.3 From b1f223b468ab5ff73c6a39749dbaf43f2f46a90b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 24 Jun 2014 22:22:56 +0200 Subject: commander: Default all leds to off --- src/modules/commander/commander_helper.cpp | 6 ++++++ 1 file changed, 6 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 940a04aa1..80e6861f6 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -209,12 +209,18 @@ int led_init() /* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */ (void)ioctl(leds, LED_ON, LED_BLUE); + /* switch blue off */ + led_off(LED_BLUE); + /* we consider the amber led mandatory */ if (ioctl(leds, LED_ON, LED_AMBER)) { warnx("Amber LED: ioctl fail\n"); return ERROR; } + /* switch amber off */ + led_off(LED_AMBER); + /* then try RGB LEDs, this can fail on FMUv1*/ rgbleds = open(RGBLED_DEVICE_PATH, 0); -- cgit v1.2.3 From bdf1b9274c369ddfd52bcb92952f2467abd7c26f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 25 Jun 2014 17:56:11 +0200 Subject: commander: modes fallback and reject messages fixed --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++++++++++---- 1 file changed, 26 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..ec4273fc3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1613,10 +1613,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTL print_reject_mode(status, "POSCTL"); } + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1627,7 +1627,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin print_reject_mode(status, "ALTCTL"); } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1639,28 +1639,50 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_RTL"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_LOITER"); + } else { res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } + + print_reject_mode(status, "AUTO_MISSION"); } - // else fallback to ALTCTL (POSCTL likely will not work too) + // fallback to POSCTL + res = main_state_transition(status, MAIN_STATE_POSCTL); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; -- cgit v1.2.3 From 1cd82fc89c70060947683851af28556bccd675a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5..588f48225 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,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, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,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); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,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, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } -- cgit v1.2.3 From 435cacb0a0c42b9995edc57cc33e68ff324e6106 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:08:25 +0200 Subject: commander: Make mavlink_fd in arming command non-optional --- src/modules/commander/state_machine_helper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..abb917873 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,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 int mavlink_fd = 0); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -- cgit v1.2.3 From 43c3559763841abacf5e74e15a6172026071088b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:08:25 +0200 Subject: commander: Make mavlink_fd in arming command non-optional --- src/modules/commander/state_machine_helper.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 0ddd4f05a..abb917873 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -57,7 +57,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 int mavlink_fd = 0); + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -- cgit v1.2.3 From 547f71d791d0a04001580e8371bdf59b808a3d29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:07:28 +0200 Subject: Add mavlink_fd to all commander arm transitions to provide user feedback why the arming command failed --- src/modules/commander/commander.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 65922b2a5..588f48225 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -412,7 +412,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* if HIL got enabled, reset battery status state */ if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); if (arming_res != TRANSITION_DENIED) { mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); @@ -945,8 +945,8 @@ int commander_thread_main(int argc, char *argv[]) if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) { arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { + mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); } } } @@ -1149,10 +1149,10 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); } status_changed = true; @@ -1163,7 +1163,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, ARMING_STATE_STANDBY, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1217,7 +1217,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); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed); + arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); stick_off_counter = 0; } else { @@ -1239,7 +1239,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } stick_on_counter = 0; @@ -1941,7 +1941,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, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -2004,7 +2004,7 @@ void *commander_low_prio_loop(void *arg) tune_negative(true); } - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); break; } -- cgit v1.2.3 From a1132580c55512a56ec7646f757099409ebad781 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:11:39 +0200 Subject: commander: Add pre-arm check for main sensors. --- src/modules/commander/state_machine_helper.cpp | 135 +++++++++++++++++++++---- 1 file changed, 118 insertions(+), 17 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1a5a52a02..6816d46ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -46,14 +46,18 @@ #include #include #include +#include #include #include #include +#include #include #include #include #include +#include +#include #include #include @@ -66,6 +70,8 @@ #endif static const int ERROR = -1; +static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + static bool arming_state_changed = true; static bool main_state_changed = true; static bool failsafe_state_changed = true; @@ -108,18 +114,31 @@ arming_state_transition(struct vehicle_status_s *status, /// current ASSERT(ARMING_STATE_INIT == 0); ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1); - /* - * Perform an atomic state update - */ - irqstate_t flags = irqsave(); - transition_result_t ret = TRANSITION_DENIED; + arming_state_t current_arming_state = status->arming_state; + /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == status->arming_state) { + if (new_arming_state == current_arming_state) { ret = TRANSITION_NOT_CHANGED; } else { + + /* + * Get sensing state if necessary + */ + int prearm_ret = OK; + + /* only perform the check if we have to */ + if (new_arming_state == ARMING_STATE_ARMED) { + prearm_ret = prearm_check(status, mavlink_fd); + } + + /* + * Perform an atomic state update + */ + irqstate_t flags = irqsave(); + /* enforce lockdown in HIL */ if (status->hil_state == HIL_STATE_ON) { armed->lockdown = true; @@ -139,12 +158,14 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF) { + // Fail transition if pre-arm check fails + if (prearm_ret) { + valid_transition = false; + // Fail transition if we need safety switch press - if (safety->safety_switch_available && !safety->safety_off) { + } else if (safety->safety_switch_available && !safety->safety_off) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first."); - } + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -173,17 +194,15 @@ arming_state_transition(struct vehicle_status_s *status, /// current status->arming_state = new_arming_state; arming_state_changed = true; } - } - /* end of atomic state update */ - irqrestore(flags); + /* end of atomic state update */ + irqrestore(flags); + } if (ret == TRANSITION_DENIED) { - static const char *errMsg = "Invalid arming transition from %s to %s"; + static const char *errMsg = "INVAL: %s - %s"; - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); - } + mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]); warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]); } @@ -496,6 +515,88 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f return ret; } +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) +{ + warnx("PREARM"); + + int ret; + + int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + ret = ioctl(fd, ACCELIOCSELFTEST, 0); + + if (ret != OK) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + goto system_eval; + } + + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + + if (accel_scale < 9.78f || accel_scale > 9.83f) { + mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + } + + if (accel_scale > 30.0f /* m/s^2 */) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } else { + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + + if (!status->is_rotary_wing) { + int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + + if (fd < 0) { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + ret = fd; + goto system_eval; + } + + struct differential_pressure_s diff_pres; + + ret = read(fd, &diff_pres, sizeof(diff_pres)); + + if (ret == sizeof(diff_pres)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; + } + } else { + mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + /* this is frickin' fatal */ + ret = ERROR; + goto system_eval; + } + } + + warnx("PRE RES: %d", ret); + +system_eval: + close(fd); + return ret; +} + // /* -- cgit v1.2.3 From 690edbd6723746699067f69fd3c76d4d3806d9c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:14:49 +0200 Subject: Remove duplicate code in the arming check --- src/modules/commander/commander.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 588f48225..8e9352394 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1232,12 +1232,13 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { - print_reject_arm("#audio: NOT ARMING: Press safety switch first."); - } else if (status.main_state != MAIN_STATE_MANUAL) { + /* we check outside of the transition function here because the requirement + * for being in manual mode only applies to manual arming actions. + * the system can be armed in auto if armed via the GCS. + */ + if (status.main_state != MAIN_STATE_MANUAL) { print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); - } else { arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); } -- cgit v1.2.3 From b20d0c0f015fb62955f7a399b50626a0d57c5f64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 26 Jun 2014 12:36:43 +0200 Subject: commander: Remove prearm test print statements --- src/modules/commander/state_machine_helper.cpp | 4 ---- 1 file changed, 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6816d46ab..8ce724b4c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -517,8 +517,6 @@ transition_result_t failsafe_state_transition(struct vehicle_status_s *status, f int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { - warnx("PREARM"); - int ret; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -590,8 +588,6 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } } - warnx("PRE RES: %d", ret); - system_eval: close(fd); return ret; -- cgit v1.2.3 From 5330ffe458bd32bcf0e733c93fa2e659fbaaeeb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 21 Jun 2014 12:24:37 +0200 Subject: commander: detect if offboard control is lost and missing offboard states --- src/modules/commander/commander.cpp | 14 ++++++++++++++ src/modules/commander/state_machine_helper.cpp | 19 +++++++++++++++++++ 2 files changed, 33 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fab80e325..0c9ce4418 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -661,6 +661,7 @@ int commander_thread_main(int argc, char *argv[]) nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; + nav_states_str[NAVIGATION_STATE_OFFBOARD] = "OFFBOARD"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -932,6 +933,19 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + if (sp_offboard.timestamp != 0 && + sp_offboard.timestamp + OFFBOARD_TIMEOUT > hrt_absolute_time()) { + if (status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = false; + status_changed = true; + } + } else { + if (!status.offboard_control_signal_lost) { + status.offboard_control_signal_lost = true; + status_changed = true; + } + } + orb_check(telemetry_sub, &updated); if (updated) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 81b489ce3..4339a7a76 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -541,6 +541,25 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } break; + case MAIN_STATE_OFFBOARD: + /* require offboard control, otherwise stay where you are */ + if (status->offboard_control_signal_lost && !status->rc_signal_lost) { + status->failsafe = true; + + status->nav_state = NAVIGATION_STATE_POSCTL; + } else if (status->offboard_control_signal_lost && status->rc_signal_lost) { + status->failsafe = true; + + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + } else { + status->nav_state = NAVIGATION_STATE_OFFBOARD; + } default: break; } -- cgit v1.2.3 From a122e88893f4b983a6e8e2e2838f9a16f055c8d1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 27 Jun 2014 10:35:02 +0200 Subject: commander: hack to get velocity offboard control working --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0c9ce4418..e4d132c40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1776,9 +1776,9 @@ set_control_mode() case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; + control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ control_mode.flag_control_velocity_enabled = true; break; case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: -- cgit v1.2.3 From d5b5dcef24a7f5c7b2ed98081dd69a05f8d46959 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 12:01:43 -0700 Subject: Fix bugs found through compiler warnings --- src/drivers/mkblctrl/mkblctrl.cpp | 8 ++++---- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/roboclaw/RoboClaw.cpp | 10 ++++++++-- src/modules/commander/commander.cpp | 1 + src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- src/modules/gpio_led/gpio_led.c | 3 ++- src/modules/px4iofirmware/sbus.c | 10 ++++++++-- src/systemcmds/mtd/mtd.c | 8 ++++++-- src/systemcmds/tests/test_mixer.cpp | 1 + 10 files changed, 33 insertions(+), 14 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5954c40da..493678ae5 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - int set_px4mode(int px4mode); - int set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: @@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate) return OK; } -int +void MK::set_px4mode(int px4mode) { _px4mode = px4mode; } -int +void MK::set_frametype(int frametype) { _frametype = frametype; diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a4bfa18c..84ea9a3bc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -741,7 +741,7 @@ PX4FMU::task_main() } for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..b0a8527c2 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor) return _motor1Position; } else if (motor == MOTOR_2) { return _motor2Position; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } float RoboClaw::getMotorSpeed(e_motor motor) @@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor) return _motor1Speed; } else if (motor == MOTOR_2) { return _motor2Speed; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } int RoboClaw::setMotorSpeed(e_motor motor, float value) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1535967b1..3cd250458 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,6 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 1320b4668..f8b9e9fbd 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1986,7 +1986,7 @@ void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt posNED[2] = -(hgt - hgtRef); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef) { lat = latRef + posNED[0] * earthRadiusInv; lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e821089f2..69d8cfce8 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -301,7 +301,7 @@ static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, flo static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float posNED[3], float &lat, float &lon, float &hgt, float latRef, float lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 839a7890b..1fc6d1295 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -189,7 +189,8 @@ int gpio_led_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; - warnx("stop"); + // FIXME: Is this correct? changed from warnx + errx(0, "stop"); } else { errx(1, "not running"); diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 0e7dc621c..8ed69678c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -119,13 +119,19 @@ sbus_init(const char *device) bool sbus1_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'A', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool sbus2_output(uint16_t *values, uint16_t num_values) { - write(sbus_fd, 'B', 1); + // FIXME: I assume this was previously NYI? + ssize_t cBytes = num_values * sizeof(values[0]); + ssize_t cBytesWritten = write(sbus_fd, values, cBytes); + return cBytesWritten == cBytes; } bool diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a57eaafe7..8b43e54da 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,8 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) - warnx(1, "failed to set bus speed"); + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index df382e2c6..dc2c20e79 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); + return 0; } static int -- cgit v1.2.3 From 2448e2adbc22af348aa799473c30ac5f1cba9cd0 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 13:55:55 -0700 Subject: More tab fixes --- src/drivers/roboclaw/RoboClaw.cpp | 8 ++++---- src/modules/commander/commander.cpp | 2 +- src/modules/gpio_led/gpio_led.c | 4 ++-- src/systemcmds/mtd/mtd.c | 12 ++++++------ src/systemcmds/tests/test_mixer.cpp | 2 +- 5 files changed, 14 insertions(+), 14 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index b0a8527c2..fdaa7f27b 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -183,8 +183,8 @@ float RoboClaw::getMotorPosition(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Position; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } @@ -195,8 +195,8 @@ float RoboClaw::getMotorSpeed(e_motor motor) } else if (motor == MOTOR_2) { return _motor2Speed; } else { - warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); - return NAN; + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; } } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3cd250458..aa243d79b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -603,7 +603,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // XXX TODO } - return true; + return true; } int commander_thread_main(int argc, char *argv[]) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index def958c95..7758faed7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,13 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); - exit(0); + exit(0); } } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); - exit(0); + exit(0); } else { errx(1, "not running"); } diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index 8b43e54da..fcc9b8366 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,12 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) { - // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried - // that but setting the bug speed does fail all the time. Which was then exiting and the board would - // not run correctly. So changed to warnx. - warnx("failed to set bus speed"); - } + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index dc2c20e79..0b826b826 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,7 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); - return 0; + return 0; } static int -- cgit v1.2.3 From 92adbe9216c96c53d1baa4eb1e14b4ede272c080 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 29 Jun 2014 17:47:24 -0700 Subject: Fix compiler warnings --- src/drivers/boards/px4fmu-v2/px4fmu2_led.c | 2 +- src/drivers/ets_airspeed/ets_airspeed.cpp | 4 +-- src/drivers/gps/gps.cpp | 6 ++-- src/drivers/hmc5883/hmc5883.cpp | 4 +-- src/drivers/mkblctrl/mkblctrl.cpp | 19 ++++------ src/drivers/px4fmu/fmu.cpp | 12 +++---- src/drivers/px4io/px4io_serial.cpp | 2 +- src/drivers/sf0x/sf0x.cpp | 2 +- src/examples/px4_simple_app/px4_simple_app.c | 1 + src/lib/geo/geo.c | 33 +++++++++-------- src/lib/geo_lookup/geo_mag_declination.c | 31 +++++++--------- .../attitude_estimator_ekf_params.c | 2 +- .../attitude_estimator_so3_main.cpp | 21 +++-------- .../commander/accelerometer_calibration.cpp | 6 ++-- src/modules/commander/calibration_routines.cpp | 2 +- src/modules/dataman/dataman.c | 3 +- .../ekf_att_pos_estimator/estimator_23states.h | 2 +- src/modules/fw_pos_control_l1/landingslope.cpp | 16 ++++----- src/modules/fw_pos_control_l1/landingslope.h | 8 ++--- src/modules/mavlink/mavlink_main.cpp | 6 ++-- src/modules/mavlink/mavlink_main.h | 4 +-- src/modules/mavlink/mavlink_orb_subscription.cpp | 6 ++-- src/modules/mavlink/mavlink_stream.cpp | 6 +++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 +- .../position_estimator_inav_main.c | 41 ++++++++++------------ src/modules/px4iofirmware/protocol.h | 2 +- src/modules/px4iofirmware/px4io.c | 5 ++- src/modules/px4iofirmware/registers.c | 3 +- src/modules/sensors/sensors.cpp | 2 +- src/modules/systemlib/hx_stream.c | 2 +- src/modules/systemlib/mixer/mixer_multirotor.cpp | 4 +-- src/modules/systemlib/otp.c | 2 +- src/modules/systemlib/param/param.c | 2 -- src/modules/systemlib/pwm_limit/pwm_limit.c | 1 - src/systemcmds/mtd/mtd.c | 30 ++++++++-------- src/systemcmds/param/param.c | 4 +-- src/systemcmds/tests/test_conv.cpp | 2 +- src/systemcmds/tests/test_file.c | 7 ++-- src/systemcmds/tests/test_file2.c | 10 +++--- src/systemcmds/tests/test_float.c | 10 +++--- src/systemcmds/tests/test_mathlib.cpp | 4 --- src/systemcmds/tests/test_mixer.cpp | 33 ++++++----------- src/systemcmds/tests/test_ppm_loopback.c | 1 - src/systemcmds/tests/test_rc.c | 1 + src/systemcmds/tests/test_servo.c | 1 + 45 files changed, 162 insertions(+), 206 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c index a856ccb02..3c05bfa46 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu2_led.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu2_led.c @@ -54,7 +54,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index 2de7063ea..1a7e068fe 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -364,7 +364,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -389,7 +389,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 8560716e9..dd505abdb 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -357,7 +357,7 @@ GPS::task_main() } if (!_healthy) { - char *mode_str = "unknown"; + const char *mode_str = "unknown"; switch (_mode) { case GPS_DRIVER_MODE_UBX: @@ -449,7 +449,7 @@ GPS::print_info() if (_report.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, - _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); + _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0d); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); @@ -578,7 +578,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char *device_name = GPS_DEFAULT_UART_PORT; + const char *device_name = GPS_DEFAULT_UART_PORT; bool fake_gps = false; /* diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index fddba806e..b7b368a5e 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -326,9 +326,9 @@ HMC5883::HMC5883(int bus) : _range_scale(0), /* default range scale from counts to gauss */ _range_ga(1.3f), _collect_phase(false), + _class_instance(-1), _mag_topic(-1), _subsystem_pub(-1), - _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -1228,7 +1228,7 @@ HMC5883::print_info() printf("offsets (%.2f %.2f %.2f)\n", (double)_scale.x_offset, (double)_scale.y_offset, (double)_scale.z_offset); printf("scaling (%.2f %.2f %.2f) 1/range_scale %.2f range_ga %.2f\n", (double)_scale.x_scale, (double)_scale.y_scale, (double)_scale.z_scale, - (double)1.0/_range_scale, (double)_range_ga); + (double)(1.0f/_range_scale), (double)_range_ga); _reports->print_info("report queue"); } diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1c81910f6..3996b76a6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -222,15 +222,15 @@ MK::MK(int bus, const char *_device_path) : _task(-1), _t_actuators(-1), _t_actuator_armed(-1), + _motor(-1), + _px4mode(MAPPING_MK), + _frametype(FRAME_PLUS), _t_outputs(0), _t_esc_status(0), _num_outputs(0), + _primary_pwm_device(false), _motortest(false), _overrideSecurityChecks(false), - _motor(-1), - _px4mode(MAPPING_MK), - _frametype(FRAME_PLUS), - _primary_pwm_device(false), _task_should_exit(false), _armed(false), _mixers(nullptr) @@ -440,9 +440,6 @@ MK::scaling(float val, float inMin, float inMax, float outMin, float outMax) void MK::task_main() { - long update_rate_in_us = 0; - float tmpVal = 0; - /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -483,7 +480,6 @@ MK::task_main() /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); - update_rate_in_us = long(1000000 / _update_rate); /* reject faster than 500 Hz updates */ if (update_rate_in_ms < 2) { @@ -735,7 +731,6 @@ MK::mk_servo_set(unsigned int chan, short val) _retries = 0; uint8_t result[3] = { 0, 0, 0 }; uint8_t msg[2] = { 0, 0 }; - uint8_t rod = 0; uint8_t bytesToSendBL2 = 2; tmpVal = val; @@ -824,7 +819,7 @@ MK::mk_servo_set(unsigned int chan, short val) if (debugCounter == 2000) { debugCounter = 0; - for (int i = 0; i < _num_outputs; i++) { + for (unsigned int i = 0; i < _num_outputs; i++) { if (Motor[i].State & MOTOR_STATE_PRESENT_MASK) { fprintf(stderr, "[mkblctrl] #%i:\tVer: %i\tVal: %i\tCurr: %i\tMaxPWM: %i\tTemp: %i\tState: %i\n", i, Motor[i].Version, Motor[i].SetPoint, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature, Motor[i].State); } @@ -1169,7 +1164,7 @@ mk_new_mode(int update_rate, int motorcount, bool motortest, int px4mode, int fr } int -mk_start(unsigned motors, char *device_path) +mk_start(unsigned motors, const char *device_path) { int ret; @@ -1228,7 +1223,7 @@ mkblctrl_main(int argc, char *argv[]) bool overrideSecurityChecks = false; bool showHelp = false; bool newMode = false; - char *devicepath = ""; + const char *devicepath = ""; /* * optional parameters diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 84ea9a3bc..0a4635728 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -240,8 +240,6 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _control_subs({-1}), - _poll_fds_num(0), _armed_sub(-1), _outputs_pub(-1), _num_outputs(0), @@ -252,10 +250,12 @@ PX4FMU::PX4FMU() : _mixers(nullptr), _groups_required(0), _groups_subscribed(0), - _failsafe_pwm({0}), - _disarmed_pwm({0}), - _num_failsafe_set(0), - _num_disarmed_set(0) + _control_subs{-1}, + _poll_fds_num(0), + _failsafe_pwm{0}, + _disarmed_pwm{0}, + _num_failsafe_set(0), + _num_disarmed_set(0) { for (unsigned i = 0; i < _max_actuators; i++) { _min_pwm[i] = PWM_DEFAULT_MIN; diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp index 3b210ac59..c39494fb0 100644 --- a/src/drivers/px4io/px4io_serial.cpp +++ b/src/drivers/px4io/px4io_serial.cpp @@ -639,7 +639,7 @@ PX4IO_serial::_do_interrupt() if (_rx_dma_status == _dma_status_waiting) { /* verify that the received packet is complete */ - int length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); + size_t length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma); if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) { perf_count(_pc_badidle); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 9109af14f..bca1715fa 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -616,7 +616,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 44e6dc7f3..4e9f099ed 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 9a24ff50e..212e33ff8 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -82,8 +82,8 @@ __EXPORT void map_projection_project(struct map_projection_reference_s *ref, dou __EXPORT void map_projection_reproject(struct map_projection_reference_s *ref, float x, float y, double *lat, double *lon) { - float x_rad = x / CONSTANTS_RADIUS_OF_EARTH; - float y_rad = y / CONSTANTS_RADIUS_OF_EARTH; + double x_rad = x / CONSTANTS_RADIUS_OF_EARTH; + double y_rad = y / CONSTANTS_RADIUS_OF_EARTH; double c = sqrtf(x_rad * x_rad + y_rad * y_rad); double sin_c = sin(c); double cos_c = cos(c); @@ -146,7 +146,6 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double double lat_next_rad = lat_next * M_DEG_TO_RAD; double lon_next_rad = lon_next * M_DEG_TO_RAD; - double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; /* conscious mix of double and float trig function to maximize speed and efficiency */ @@ -174,8 +173,8 @@ __EXPORT void add_vector_to_global_position(double lat_now, double lon_now, floa double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; - *lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; - *lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; + *lat_res = (lat_now_rad + (double)v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG; + *lon_res = (lon_now_rad + (double)v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG; } // Additional functions - @author Doug Weibel @@ -197,7 +196,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_start == 0.0 || lon_start == 0.0 || lat_end == 0.0d || lon_end == 0.0d) { return return_value; } bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); @@ -212,7 +211,7 @@ __EXPORT int get_distance_to_line(struct crosstrack_error_s *crosstrack_error, d } dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); - crosstrack_error->distance = (dist_to_end) * sin(bearing_diff); + crosstrack_error->distance = (dist_to_end) * sinf(bearing_diff); if (sin(bearing_diff) >= 0) { crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F); @@ -248,7 +247,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do crosstrack_error->bearing = 0.0f; // Return error if arguments are bad - if (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) { return return_value; } + if (lat_now == 0.0 || lon_now == 0.0 || lat_center == 0.0 || lon_center == 0.0 || radius == 0.0f) { return return_value; } if (arc_sweep >= 0) { @@ -296,14 +295,14 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // as this function generally will not be called repeatedly when we are out of the sector. // TO DO - this is messed up and won't compile - float start_disp_x = radius * sin(arc_start_bearing); - float start_disp_y = radius * cos(arc_start_bearing); - float end_disp_x = radius * sin(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cos(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0d; - float lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0d; - float lon_end = lon_now + end_disp_x / 111111.0d; - float lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0d; + float start_disp_x = radius * sinf(arc_start_bearing); + float start_disp_y = radius * cosf(arc_start_bearing); + float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); + float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); + float lon_start = lon_now + start_disp_x / 111111.0f; + float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = lon_now + end_disp_x / 111111.0f; + float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -337,7 +336,7 @@ __EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now double d_lat = x_rad - current_x_rad; double d_lon = y_rad - current_y_rad; - double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad); + double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0) * cos(current_x_rad) * cos(x_rad); double c = 2 * atan2(sqrt(a), sqrt(1 - a)); float dxy = CONSTANTS_RADIUS_OF_EARTH * c; diff --git a/src/lib/geo_lookup/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index 09eac38f4..c41d52606 100644 --- a/src/lib/geo_lookup/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c @@ -54,24 +54,19 @@ static const int8_t declination_table[13][37] = \ { - 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ - -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ - -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ - 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ - -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ - 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ - 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ - -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ - -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ - 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ - 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ - 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ - 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ - 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ - -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ - 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ - 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ - 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 + { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, }; static float get_lookup_table_val(unsigned lat, unsigned lon); diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 49a892609..bc0e3b93a 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -101,7 +101,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r3, &(p->r[3])); param_get(h->mag_decl, &(p->mag_decl)); - p->mag_decl *= M_PI / 180.0f; + p->mag_decl *= M_PI_F / 180.0f; param_get(h->acc_comp, &(p->acc_comp)); diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e55b01160..e49027e47 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -392,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl */ int attitude_estimator_so3_thread_main(int argc, char *argv[]) { - const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - //! Time constant float dt = 0.005f; @@ -438,11 +436,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs /* keep track of sensor updates */ @@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); + warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); } } else { @@ -523,12 +519,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; - uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ if (sensor_last_timestamp[0] != raw.timestamp) { - update_vect[0] = 1; - sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); sensor_last_timestamp[0] = raw.timestamp; } @@ -538,8 +531,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update accelerometer measurements */ if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { - update_vect[1] = 1; - sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); sensor_last_timestamp[1] = raw.accelerometer_timestamp; } @@ -549,8 +540,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* update magnetometer measurements */ if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { - update_vect[2] = 1; - sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); sensor_last_timestamp[2] = raw.magnetometer_timestamp; } @@ -569,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_start = hrt_absolute_time(); - // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], @@ -609,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* due to inputs or numerical failure the output is invalid, skip it */ // Due to inputs or numerical failure the output is invalid warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); - warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); - warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); // Don't publish anything continue; } diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 7180048ff..24da452b1 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -158,6 +158,8 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { + int fd; + mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); struct accel_scale accel_scale = { @@ -172,7 +174,7 @@ int do_accel_calibration(int mavlink_fd) int res = OK; /* reset all offsets to zero and all scales to one */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -223,7 +225,7 @@ int do_accel_calibration(int mavlink_fd) if (res == OK) { /* apply new scaling and offsets */ - int fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, 0); res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index be38ea104..9d79124e7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -170,7 +170,7 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], float aA, aB, aC, nA, nB, nC, dA, dB, dC; //Iterate N times, ignore stop condition. - int n = 0; + unsigned int n = 0; while (n < max_iterations) { n++; diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index 1a65313e8..406293bda 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -50,6 +50,7 @@ #include #include "dataman.h" +#include /** * data manager app start / stop handling function @@ -187,7 +188,7 @@ create_work_item(void) if (item) { item->first = 1; lock_queue(&g_free_q); - for (int i = 1; i < k_work_item_allocation_chunk_size; i++) { + for (size_t i = 1; i < k_work_item_allocation_chunk_size; i++) { (item + i)->first = 0; sq_addfirst(&(item + i)->link, &(g_free_q.q)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index 7aad849f9..15ceb57c0 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_23states.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -234,7 +234,7 @@ int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); -void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); +void quat2Tbn(Mat3f &TBodyNed, const float (&quat)[4]); void calcEarthRateNED(Vector3f &omega, float latitude); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index 8ce465fe8..42e00da05 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -46,16 +46,16 @@ #include #include -void Landingslope::update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt) +void Landingslope::update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new) { - _landing_slope_angle_rad = landing_slope_angle_rad; - _flare_relative_alt = flare_relative_alt; - _motor_lim_relative_alt = motor_lim_relative_alt; - _H1_virt = H1_virt; + _landing_slope_angle_rad = landing_slope_angle_rad_new; + _flare_relative_alt = flare_relative_alt_new; + _motor_lim_relative_alt = motor_lim_relative_alt_new; + _H1_virt = H1_virt_new; calculateSlopeValues(); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index b54fd501c..a5975ad43 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -123,10 +123,10 @@ public: float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); - void update(float landing_slope_angle_rad, - float flare_relative_alt, - float motor_lim_relative_alt, - float H1_virt); + void update(float landing_slope_angle_rad_new, + float flare_relative_alt_new, + float motor_lim_relative_alt_new, + float H1_virt_new); inline float landing_slope_angle_rad() {return _landing_slope_angle_rad;} diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9a5e31ef4..43acee96f 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2266,13 +2266,13 @@ Mavlink::start(int argc, char *argv[]) } void -Mavlink::status() +Mavlink::display_status() { warnx("running"); } int -Mavlink::stream(int argc, char *argv[]) +Mavlink::stream_command(int argc, char *argv[]) { const char *device_name = DEFAULT_DEVICE_NAME; float rate = -1.0f; @@ -2360,7 +2360,7 @@ int mavlink_main(int argc, char *argv[]) // mavlink::g_mavlink->status(); } else if (!strcmp(argv[1], "stream")) { - return Mavlink::stream(argc, argv); + return Mavlink::stream_command(argc, argv); } else { usage(); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 6777d56c3..5f4117ae5 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,9 +123,9 @@ public: /** * Display the mavlink status. */ - void status(); + void display_status(); - static int stream(int argc, char *argv[]); + static int stream_command(int argc, char *argv[]); static int instance_count(); diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 901fa8f05..734f0903a 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -47,10 +47,10 @@ #include "mavlink_orb_subscription.h" MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : - _fd(orb_subscribe(_topic)), - _published(false), + next(nullptr), _topic(topic), - next(nullptr) + _fd(orb_subscribe(_topic)), + _published(false) { } diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp index 5ec30bd33..ed6776e5c 100644 --- a/src/modules/mavlink/mavlink_stream.cpp +++ b/src/modules/mavlink/mavlink_stream.cpp @@ -43,7 +43,11 @@ #include "mavlink_stream.h" #include "mavlink_main.h" -MavlinkStream::MavlinkStream() : _interval(1000000), _last_sent(0), _channel(MAVLINK_COMM_0), next(nullptr) +MavlinkStream::MavlinkStream() : + _last_sent(0), + _channel(MAVLINK_COMM_0), + _interval(1000000), + next(nullptr) { } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 09960d106..c24c172af 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -545,7 +545,6 @@ MulticopterPositionControl::task_main() hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; - const float pos_ctl_dz = 0.05f; math::Vector<3> sp_move_rate; sp_move_rate.zero(); @@ -862,7 +861,7 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_velocity_enabled) { /* limit max tilt */ - if (thr_min >= 0.0f && tilt_max < M_PI / 2 - 0.05f) { + if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) { /* absolute horizontal thrust */ float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length(); 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 62cee145e..9870ebd05 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -179,15 +179,21 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(1); } -void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) +static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) { FILE *f = fopen("/fs/microsd/inav.log", "a"); if (f) { char *s = malloc(256); - unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]); + unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", + hrt_absolute_time(), msg, (double)dt, + (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], + (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); - n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); + n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", + (double)acc[0], (double)acc[1], (double)acc[2], + (double)corr_gps[0][0], (double)corr_gps[1][0], (double)corr_gps[2][0], (double)corr_gps[0][1], (double)corr_gps[1][1], (double)corr_gps[2][1], + (double)w_xy_gps_p, (double)w_xy_gps_v); fwrite(s, 1, n, f); free(s); } @@ -261,9 +267,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; - /* acceleration in NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; - /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float acc[] = { 0.0f, 0.0f, 0.0f }; // N E D float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame @@ -285,7 +288,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) - hrt_abstime xy_src_time = 0; // time of last available position data bool gps_valid = false; // GPS is valid bool sonar_valid = false; // sonar is valid @@ -370,8 +372,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { wait_baro = false; baro_offset /= (float) baro_init_cnt; - warnx("baro offs: %.2f", baro_offset); - mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset); + warnx("baro offs: %.2f", (double)baro_offset); + mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); local_pos.z_valid = true; local_pos.v_z_valid = true; } @@ -475,7 +477,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; flow_prev = flow.flow_timestamp; - if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) { + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7f && flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; @@ -497,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) sonar_valid_time = t; sonar_valid = true; local_pos.surface_bottom_timestamp = t; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", surface_offset); + mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); } } else { @@ -510,7 +512,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7) { + if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && att.R[2][2] > 0.7f) { /* distance to surface */ float flow_dist = dist_bottom / att.R[2][2]; /* check if flow if too large for accurate measurements */ @@ -558,7 +560,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* under ideal conditions, on 1m distance assume EPH = 10cm */ - eph_flow = 0.1 / w_flow; + eph_flow = 0.1f / w_flow; flow_valid = true; @@ -661,8 +663,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize projection */ map_projection_init(&ref, lat, lon); - warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); - mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); + mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); } } @@ -746,10 +748,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* increase EPH/EPV on each step */ if (eph < max_eph_epv) { - eph *= 1.0 + dt; + eph *= 1.0f + dt; } if (epv < max_eph_epv) { - epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + epv += 0.005f * dt; // add 1m to EPV each 200s (baro drift) } /* use GPS if it's valid and reference position initialized */ @@ -758,11 +760,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* use flow if it's valid and (accurate or no GPS available) */ bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); - /* try to estimate position during some time after position sources lost */ - if (use_gps_xy || use_flow) { - xy_src_time = t; - } - bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 7471faec7..d5a6b1ec4 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -312,7 +312,7 @@ struct IOPacket { #define PKT_COUNT(_p) ((_p).count_code & PKT_COUNT_MASK) #define PKT_CODE(_p) ((_p).count_code & PKT_CODE_MASK) -#define PKT_SIZE(_p) ((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p))) +#define PKT_SIZE(_p) ((size_t)((uint8_t *)&((_p).regs[PKT_COUNT(_p)]) - ((uint8_t *)&(_p)))) static const uint8_t crc8_tab[256] __attribute__((unused)) = { diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index d4c25911e..cd134ccb4 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -37,6 +37,7 @@ */ #include +#include #include // required for task_create #include @@ -303,14 +304,12 @@ user_start(int argc, char *argv[]) */ if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { - struct mallinfo minfo = mallinfo(); - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)minfo.mxordblk); + (unsigned)mallinfo().mxordblk); last_debug_time = hrt_absolute_time(); } } diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index db1836f4a..50108ea1b 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -119,7 +119,6 @@ uint16_t r_page_raw_rc_input[] = [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_FRAME_COUNT] = 0, [PX4IO_P_RAW_LOST_FRAME_COUNT] = 0, - [PX4IO_P_RAW_RC_DATA] = 0, [PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + PX4IO_RC_INPUT_CHANNELS)] = 0 }; @@ -670,7 +669,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] == UINT8_MAX) { disabled = true; - } else if ((int)(conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]) < 0 || conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { + } else if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= PX4IO_RC_MAPPED_CONTROL_CHANNELS) { count++; } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 16fcb4c26..8908adf4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -649,7 +649,7 @@ Sensors::parameters_update() if (!isfinite(tmpScaleFactor) || (tmpRevFactor < 0.000001f) || (tmpRevFactor > 0.2f)) { - warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i])); + warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, (double)tmpScaleFactor, (int)(_parameters.rev[i])); /* scaling factors do not make sense, lock them down */ _parameters.scaling_factor[i] = 0.0f; rc_valid = false; diff --git a/src/modules/systemlib/hx_stream.c b/src/modules/systemlib/hx_stream.c index 8e9c2bfcf..52ae77de5 100644 --- a/src/modules/systemlib/hx_stream.c +++ b/src/modules/systemlib/hx_stream.c @@ -63,7 +63,7 @@ struct hx_stream { /* TX state */ int fd; bool tx_error; - uint8_t *tx_buf; + const uint8_t *tx_buf; unsigned tx_resid; uint32_t tx_crc; enum { diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 092c0e2b0..4b22a46d0 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -208,7 +208,6 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl char geomname[8]; int s[4]; int used; - const char *end = buf + buflen; /* enforce that the mixer ends with space or a new line */ for (int i = buflen - 1; i >= 0; i--) { @@ -302,7 +301,6 @@ MultirotorMixer::mix(float *outputs, unsigned space) //lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3))); float min_out = 0.0f; float max_out = 0.0f; - float scale_yaw = 1.0f; /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { @@ -327,7 +325,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) } /* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */ - if (min_out < 0.0) { + if (min_out < 0.0f) { float scale_in = thrust / (thrust - min_out); /* mix again with adjusted controls */ diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c index 695574fdc..0548a9f7d 100644 --- a/src/modules/systemlib/otp.c +++ b/src/modules/systemlib/otp.c @@ -133,7 +133,7 @@ int lock_otp(void) // COMPLETE, BUSY, or other flash error? -int F_GetStatus(void) +static int F_GetStatus(void) { int fs = F_COMPLETE; diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 7a499ca72..e44e6cdb0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -96,8 +96,6 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; -static sem_t param_sem = { .semcount = 1 }; - /** lock the parameter store */ static void param_lock(void) diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index ea5ba9e52..c733a53d7 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -97,7 +97,6 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ } unsigned progress; - uint16_t temp_pwm; /* then set effective_pwm based on state */ switch (limit->state) { diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index fcc9b8366..a925cdd40 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -91,7 +91,7 @@ static void mtd_test(void); static void mtd_erase(char *partition_names[], unsigned n_partitions); static void mtd_readtest(char *partition_names[], unsigned n_partitions); static void mtd_rwtest(char *partition_names[], unsigned n_partitions); -static void mtd_print_info(); +static void mtd_print_info(void); static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks, unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions); @@ -104,6 +104,16 @@ static unsigned n_partitions_current = 0; static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"}; static const int n_partitions_default = sizeof(partition_names_default) / sizeof(partition_names_default[0]); +static void +mtd_status(void) +{ + if (!attached) + errx(1, "MTD driver not started"); + + mtd_print_info(); + exit(0); +} + int mtd_main(int argc, char *argv[]) { if (argc >= 2) { @@ -355,7 +365,7 @@ static ssize_t mtd_get_partition_size(void) return partsize; } -void mtd_print_info() +void mtd_print_info(void) { if (!attached) exit(1); @@ -385,16 +395,6 @@ mtd_test(void) exit(1); } -void -mtd_status(void) -{ - if (!attached) - errx(1, "MTD driver not started"); - - mtd_print_info(); - exit(0); -} - void mtd_erase(char *partition_names[], unsigned n_partitions) { @@ -428,7 +428,7 @@ mtd_readtest(char *partition_names[], unsigned n_partitions) uint8_t v[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; + ssize_t count = 0; printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDONLY); if (fd == -1) { @@ -459,8 +459,8 @@ mtd_rwtest(char *partition_names[], unsigned n_partitions) uint8_t v[128], v2[128]; for (uint8_t i = 0; i < n_partitions; i++) { - uint32_t count = 0; - off_t offset = 0; + ssize_t count = 0; + off_t offset = 0; printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); int fd = open(partition_names[i], O_RDWR); if (fd == -1) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index d92ee88ef..328205e29 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -63,7 +63,7 @@ static void do_show(const char* search_string); static void do_show_print(void *arg, param_t param); static void do_set(const char* name, const char* val, bool fail_on_not_found); static void do_compare(const char* name, const char* vals[], unsigned comparisons); -static void do_reset(); +static void do_reset(void); int param_main(int argc, char *argv[]) @@ -416,7 +416,7 @@ do_compare(const char* name, const char* vals[], unsigned comparisons) } static void -do_reset() +do_reset(void) { param_reset_all(); diff --git a/src/systemcmds/tests/test_conv.cpp b/src/systemcmds/tests/test_conv.cpp index 50dece816..0ecdc8eb9 100644 --- a/src/systemcmds/tests/test_conv.cpp +++ b/src/systemcmds/tests/test_conv.cpp @@ -65,7 +65,7 @@ int test_conv(int argc, char *argv[]) float f = i/10000.0f; float fres = REG_TO_FLOAT(FLOAT_TO_REG(f)); if (fabsf(f - fres) > 0.0001f) { - warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres); + warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), (double)fres); return 1; } } diff --git a/src/systemcmds/tests/test_file.c b/src/systemcmds/tests/test_file.c index 96be1e8df..86f23b485 100644 --- a/src/systemcmds/tests/test_file.c +++ b/src/systemcmds/tests/test_file.c @@ -102,7 +102,7 @@ test_file(int argc, char *argv[]) } /* perform tests for a range of chunk sizes */ - unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; + int chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { @@ -116,7 +116,7 @@ test_file(int argc, char *argv[]) uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); /* fill write buffer with known values */ - for (int i = 0; i < sizeof(write_buf); i++) { + for (size_t i = 0; i < sizeof(write_buf); i++) { /* this will wrap, but we just need a known value with spacing */ write_buf[i] = i+11; } @@ -224,9 +224,6 @@ test_file(int argc, char *argv[]) return 1; } - /* compare value */ - bool compare_ok = true; - for (int j = 0; j < chunk_sizes[c]; j++) { if (read_buf[j] != write_buf[j]) { warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); diff --git a/src/systemcmds/tests/test_file2.c b/src/systemcmds/tests/test_file2.c index ef555f6c3..8db3ea5ae 100644 --- a/src/systemcmds/tests/test_file2.c +++ b/src/systemcmds/tests/test_file2.c @@ -49,6 +49,8 @@ #include #include +#include "tests.h" + #define FLAG_FSYNC 1 #define FLAG_LSEEK 2 @@ -85,9 +87,9 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t buffer[j] = get_value(ofs); ofs++; } - if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { - printf("write failed at offset %u\n", ofs); - exit(1); + if (write(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { + printf("write failed at offset %u\n", ofs); + exit(1); } if (flags & FLAG_FSYNC) { fsync(fd); @@ -116,7 +118,7 @@ static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t printf("read ofs=%u\r", ofs); } counter++; - if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) { + if (read(fd, buffer, sizeof(buffer)) != (int)sizeof(buffer)) { printf("read failed at offset %u\n", ofs); exit(1); } diff --git a/src/systemcmds/tests/test_float.c b/src/systemcmds/tests/test_float.c index 4921c9bbb..e33cc6ef7 100644 --- a/src/systemcmds/tests/test_float.c +++ b/src/systemcmds/tests/test_float.c @@ -94,7 +94,7 @@ int test_float(int argc, char *argv[]) printf("\t success: asinf(1.0f) == 1.57079f\n"); } else { - printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", asinf_one); + printf("\t FAIL: asinf(1.0f) != 1.57079f, result: %f\n", (double)asinf_one); ret = -1; } @@ -128,7 +128,7 @@ int test_float(int argc, char *argv[]) float sinf_zero_one = sinf(0.1f); - if (fabs(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { + if (fabsf(sinf_zero_one - 0.0998334166f) < FLT_EPSILON) { printf("\t success: sinf(0.1f) == 0.09983f\n"); } else { @@ -155,7 +155,7 @@ int test_float(int argc, char *argv[]) } char sbuf[30]; - sprintf(sbuf, "%8.4f", 0.553415f); + sprintf(sbuf, "%8.4f", (double)0.553415f); if (sbuf[0] == ' ' && sbuf[1] == ' ' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -166,7 +166,7 @@ int test_float(int argc, char *argv[]) ret = -5; } - sprintf(sbuf, "%8.4f", -0.553415f); + sprintf(sbuf, "%8.4f", (double)-0.553415f); if (sbuf[0] == ' ' && sbuf[1] == '-' && sbuf[2] == '0' && sbuf[3] == '.' && sbuf[4] == '5' && sbuf[5] == '5' @@ -205,7 +205,7 @@ int test_float(int argc, char *argv[]) printf("\t success: (float) 1.55f == 1.55 (double)\n"); } else { - printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", f1); + printf("\t FAIL: (float) 1.55f != 1.55 (double), result: %8.4f\n", (double)f1); ret = -8; } diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 693a208ba..00c649c75 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -52,10 +52,6 @@ using namespace math; -const char* formatResult(bool res) { - return res ? "OK" : "ERROR"; -} - int test_mathlib(int argc, char *argv[]) { warnx("testing mathlib"); diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index 0b826b826..8ab8fa2d6 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -81,7 +81,7 @@ int test_mixer(int argc, char *argv[]) warnx("testing mixer"); - char *filename = "/etc/mixers/IO_pass.mix"; + const char *filename = "/etc/mixers/IO_pass.mix"; if (argc > 1) filename = argv[1]; @@ -100,8 +100,6 @@ int test_mixer(int argc, char *argv[]) * e.g. on PX4IO. */ - unsigned nused = 0; - const unsigned chunk_size = 64; MixerGroup mixer_group(mixer_callback, 0); @@ -124,7 +122,6 @@ int test_mixer(int argc, char *argv[]) return 1; /* FIRST mark the mixer as invalid */ - bool mixer_ok = false; /* THEN actually delete it */ mixer_group.reset(); char mixer_text[256]; /* large enough for one mixer */ @@ -140,7 +137,6 @@ int test_mixer(int argc, char *argv[]) /* check for overflow - this would be really fatal */ if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) { - bool mixer_ok = false; return 1; } @@ -156,15 +152,6 @@ int test_mixer(int argc, char *argv[]) /* if anything was parsed */ if (resid != mixer_text_length) { - - /* only set mixer ok if no residual is left over */ - if (resid == 0) { - mixer_ok = true; - } else { - /* not yet reached the end of the mixer, set as not ok */ - mixer_ok = false; - } - warnx("used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ @@ -192,7 +179,7 @@ int test_mixer(int argc, char *argv[]) should_arm = true; /* run through arming phase */ - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = 0.1f; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -213,7 +200,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (hrt_elapsed_time(&starttime) < INIT_TIME_US && @@ -228,7 +215,7 @@ int test_mixer(int argc, char *argv[]) return 1; } - //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]); + //printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, (double)outputs_unlimited[i], (double)outputs[i], (int)r_page_servos[i]); } usleep(sleep_quantum_us); sleepcount++; @@ -244,7 +231,7 @@ int test_mixer(int argc, char *argv[]) for (int j = -jmax; j <= jmax; j++) { - for (int i = 0; i < output_max; i++) { + for (unsigned i = 0; i < output_max; i++) { actuator_controls[i] = j/10.0f + 0.1f * i; r_page_servo_disarmed[i] = PWM_LOWEST_MIN; r_page_servo_control_min[i] = PWM_DEFAULT_MIN; @@ -257,11 +244,11 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); warnx("mixed %d outputs (max %d)", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } @@ -282,7 +269,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* check mixed outputs to be zero during init phase */ if (r_page_servos[i] != r_page_servo_disarmed[i]) { @@ -316,7 +303,7 @@ int test_mixer(int argc, char *argv[]) pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit); //warnx("mixed %d outputs (max %d), values:", mixed, output_max); - for (int i = 0; i < mixed; i++) + for (unsigned i = 0; i < mixed; i++) { /* predict value */ servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f; @@ -333,7 +320,7 @@ int test_mixer(int argc, char *argv[]) /* check post ramp phase */ if (hrt_elapsed_time(&starttime) > RAMP_TIME_US && fabsf(servo_predicted[i] - r_page_servos[i]) > 2) { - printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]); + printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, (double)outputs[i], servo_predicted[i], (int)r_page_servos[i]); warnx("mixer violated predicted value"); return 1; } diff --git a/src/systemcmds/tests/test_ppm_loopback.c b/src/systemcmds/tests/test_ppm_loopback.c index b9041b013..addd57bea 100644 --- a/src/systemcmds/tests/test_ppm_loopback.c +++ b/src/systemcmds/tests/test_ppm_loopback.c @@ -65,7 +65,6 @@ int test_ppm_loopback(int argc, char *argv[]) int _rc_sub = orb_subscribe(ORB_ID(input_rc)); int servo_fd, result; - servo_position_t data[PWM_OUTPUT_MAX_CHANNELS]; servo_position_t pos; servo_fd = open(PWM_OUTPUT_DEVICE_PATH, O_RDWR); diff --git a/src/systemcmds/tests/test_rc.c b/src/systemcmds/tests/test_rc.c index 57c0e7f4c..c9f9ef439 100644 --- a/src/systemcmds/tests/test_rc.c +++ b/src/systemcmds/tests/test_rc.c @@ -52,6 +52,7 @@ #include #include #include +#include #include #include "tests.h" diff --git a/src/systemcmds/tests/test_servo.c b/src/systemcmds/tests/test_servo.c index ef8512bf5..9c6951ca2 100644 --- a/src/systemcmds/tests/test_servo.c +++ b/src/systemcmds/tests/test_servo.c @@ -51,6 +51,7 @@ #include #include +#include #include -- cgit v1.2.3 From 3f6851b9879c2e4d764926a3bc29ff800a17b73d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 12:19:08 +0200 Subject: Re-send the RC config warnings once the GCS is connected, fixed compile warnings --- .../commander/accelerometer_calibration.cpp | 3 ++- src/modules/commander/calibration_routines.cpp | 7 +++--- src/modules/commander/commander.cpp | 26 +++++++++++++++++----- 3 files changed, 27 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 24da452b1..be465ab76 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -131,6 +131,7 @@ #include #include #include +#include #include #include #include @@ -526,7 +527,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0f) { + if (fabsf(det) < FLT_EPSILON) { return ERROR; // Singular matrix } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 9d79124e7..43f341ae7 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -40,6 +40,7 @@ */ #include +#include #include "calibration_routines.h" @@ -179,9 +180,9 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], aA = Q2 + 16.0f * (A2 - 2.0f * A * x_sum + x_sum2); aB = Q2 + 16.0f * (B2 - 2.0f * B * y_sum + y_sum2); aC = Q2 + 16.0f * (C2 - 2.0f * C * z_sum + z_sum2); - aA = (aA == 0.0f) ? 1.0f : aA; - aB = (aB == 0.0f) ? 1.0f : aB; - aC = (aC == 0.0f) ? 1.0f : aC; + aA = (fabsf(aA) < FLT_EPSILON) ? 1.0f : aA; + aB = (fabsf(aB) < FLT_EPSILON) ? 1.0f : aB; + aC = (fabsf(aC) < FLT_EPSILON) ? 1.0f : aC; //Compute next iteration nA = A - ((F2 + 16.0f * (B * XY + C * XZ + x_sum * (-A2 - Q0) + A * (x_sum2 + Q1 - C * z_sum - B * y_sum))) / aA); diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bb42889ea..ef4c18ea6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -371,16 +371,16 @@ void print_status() static orb_advert_t status_pub; -transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy) +transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char *armedBy) { transition_result_t arming_res = TRANSITION_NOT_CHANGED; // Transition the armed state. By passing mavlink_fd to arming_state_transition it will // output appropriate error messages if the state cannot transition. - arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd); + arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd_local); if (arming_res == TRANSITION_CHANGED && mavlink_fd) { - mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); + mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy); } else if (arming_res == TRANSITION_DENIED) { tune_negative(true); @@ -507,7 +507,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); + mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + (double)cmd->param1, + (double)cmd->param2, + (double)cmd->param3, + (double)cmd->param4, + (double)cmd->param5, + (double)cmd->param6, + (double)cmd->param7); } } break; @@ -752,7 +759,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime last_auto_state_valid = 0; bool status_changed = true; bool param_init_forced = true; @@ -862,6 +868,7 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; + bool system_checked = false; while (!thread_should_exit) { @@ -915,6 +922,15 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } + /* Perform system checks (again) once params are loaded and MAVLink is up. */ + if (!system_checked && mavlink_fd && + (telemetry.heartbeat_time > 0) && + (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { + + (void)rc_calibration_check(mavlink_fd); + system_checked = true; + } + orb_check(sp_man_sub, &updated); if (updated) { -- cgit v1.2.3 From 0a159e1a2490e5ec7f368d938e2b0fd365d2731e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 30 Jun 2014 12:31:29 +0200 Subject: mavlink, commander: bugs fixed --- src/modules/commander/commander.cpp | 2 +- src/modules/mavlink/mavlink_main.h | 10 ++-------- 2 files changed, 3 insertions(+), 9 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ff1195c9..bba14ea27 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1407,7 +1407,7 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.mission_finished); + mission_result.finished); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index b4184b21c..67ef8d00f 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -182,7 +182,7 @@ public: int send_statustext(const char *string); int send_statustext(enum MAV_SEVERITY severity, const char *string); - MavlinkStream * get_streams() { return _streams; } const + MavlinkStream * get_streams() const { return _streams; } float get_rate_mult(); @@ -193,7 +193,7 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } - bool message_buffer_write(void *ptr, int size); + bool message_buffer_write(const void *ptr, int size); void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -322,10 +322,6 @@ private: void mavlink_update_system(); - uint8_t get_system_id(); - - uint8_t get_component_id(); - int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); @@ -338,8 +334,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(const void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); -- cgit v1.2.3 From 799f568ab48ca3446b24f41290589f7f3c39ef0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 14:33:56 +0200 Subject: commander: Minor checks / improvements to power enforce patch --- src/modules/commander/commander.cpp | 4 +++- src/modules/commander/state_machine_helper.cpp | 16 +++++----------- 2 files changed, 8 insertions(+), 12 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 507f18e24..a15712a91 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -974,8 +974,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = false; } else { status.condition_power_input_valid = true; - status.avionics_power_rail_voltage = system_power.voltage5V_v; } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1997729d4..7a61d481b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -141,22 +141,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); - } - + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { - - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); - } + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f)) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From bbdf2bc07aaaa363ef58f15831511c64c2d856bf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 16:58:05 +0200 Subject: commander: Notify negative on manual arming fails --- src/modules/commander/commander.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e4a709ef6..efa26eb97 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1342,6 +1342,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + tune_negative(true); } /* evaluate the main state machine according to mode switches */ -- cgit v1.2.3 From fb98251e4d08d351c7ba404e3816958435c57515 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 1 Jul 2014 09:52:36 +0200 Subject: Warning fixes in commander --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 244513375..423ce2f23 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,7 +182,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && (status->avionics_power_rail_voltage < 4.9f)) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } @@ -638,7 +638,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); + fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); -- cgit v1.2.3 From d4eae37e478860a59e21f3caceb3d8fc28f9fa7c Mon Sep 17 00:00:00 2001 From: Antonio Sanniravong Date: Fri, 4 Jul 2014 21:00:05 -0400 Subject: Commander: Only consider latest active data link heartbeat for timeout. --- src/modules/commander/commander.cpp | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..699ced1ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,6 +767,7 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; + hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -1367,19 +1368,24 @@ int commander_thread_main(int argc, char *argv[]) } /* data link check */ - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { - /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; - } + if (telemetry.heartbeat_time >= latest_heartbeat) { + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; + } - } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + /* Only consider data link with most recent heartbeat */ + latest_heartbeat = telemetry.heartbeat_time; + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = true; + status_changed = true; + } } } -- cgit v1.2.3 From b9299e68d4147845bab9ed99509b3e50b7c94ae1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 5 Jul 2014 13:35:12 -0700 Subject: Compiler warning fixes --- src/drivers/blinkm/blinkm.cpp | 2 +- src/lib/geo/geo.c | 14 ++++++------- src/modules/commander/commander.cpp | 8 ++++---- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 23 +++++++++++----------- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 4 ++-- src/modules/systemlib/cpuload.c | 2 +- src/modules/systemlib/systemlib.c | 3 +++ src/systemcmds/param/param.c | 5 +++-- 8 files changed, 33 insertions(+), 28 deletions(-) (limited to 'src/modules/commander') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 92e089217..6b14f5945 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -293,7 +293,7 @@ BlinkM::BlinkM(int bus, int blinkm) : safety_sub_fd(-1), num_of_cells(0), detected_cells_runcount(0), - t_led_color({0}), + t_led_color{0}, t_led_blink(0), led_thread_runcount(0), led_interval(1000), diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 212e33ff8..230be9281 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -297,12 +297,12 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // TO DO - this is messed up and won't compile float start_disp_x = radius * sinf(arc_start_bearing); float start_disp_y = radius * cosf(arc_start_bearing); - float end_disp_x = radius * sinf(_wrapPI(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0f; - float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = lon_now + end_disp_x / 111111.0f; - float lat_end = lat_now + end_disp_y * cosf(lat_now) / 111111.0f; + float end_disp_x = radius * sinf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float end_disp_y = radius * cosf(_wrapPI((double)(arc_start_bearing + arc_sweep))); + float lon_start = (float)lon_now + start_disp_x / 111111.0f; + float lat_start = (float)lat_now + start_disp_y * cosf(lat_now) / 111111.0f; + float lon_end = (float)lon_now + end_disp_x / 111111.0f; + float lat_end = (float)lat_now + end_disp_y * cosf(lat_now) / 111111.0f; float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); @@ -319,7 +319,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..3bf3c857d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -472,7 +472,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. // We use an float epsilon delta to test float equality. if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1); + mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); } else { @@ -634,7 +634,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("starting"); - char *main_states_str[MAIN_STATE_MAX]; + const char *main_states_str[MAIN_STATE_MAX]; main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; @@ -643,7 +643,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; - char *arming_states_str[ARMING_STATE_MAX]; + const char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[ARMING_STATE_INIT] = "INIT"; arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; arming_states_str[ARMING_STATE_ARMED] = "ARMED"; @@ -652,7 +652,7 @@ int commander_thread_main(int argc, char *argv[]) arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; - char *nav_states_str[NAVIGATION_STATE_MAX]; + const char *nav_states_str[NAVIGATION_STATE_MAX]; nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 000c02e3d..a9556f453 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -414,6 +414,17 @@ FixedwingPositionControl::FixedwingPositionControl() : _attitude_sp_pub(-1), _nav_capabilities_pub(-1), + _att(), + _att_sp(), + _nav_capabilities(), + _manual(), + _airspeed(), + _control_mode(), + _global_pos(), + _pos_sp_triplet(), + _sensor_combined(), + _range_finder(), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -433,18 +444,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - _att(), - _att_sp(), - _nav_capabilities(), - _manual(), - _airspeed(), - _control_mode(), - _global_pos(), - _pos_sp_triplet(), - _sensor_combined(), _mTecs(), - _was_pos_control_mode(false), - _range_finder() + _was_pos_control_mode(false) { _nav_capabilities.turn_distance = 0.0f; diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index e4e405227..c22e60ae0 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -56,9 +56,9 @@ class BlockOutputLimiter: public SuperBlock { public: // methods - BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) : SuperBlock(parent, name), - _isAngularLimit(isAngularLimit), + _isAngularLimit(fAngularLimit), _min(this, "MIN"), _max(this, "MAX") {}; diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c index ccc516f39..7aa2f3a5f 100644 --- a/src/modules/systemlib/cpuload.c +++ b/src/modules/systemlib/cpuload.c @@ -67,7 +67,7 @@ __EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pT __EXPORT struct system_load_s system_load; -extern FAR struct _TCB *sched_gettcb(pid_t pid); +extern FAR struct tcb_s *sched_gettcb(pid_t pid); void cpuload_initialize_once() { diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 9fff3eb88..90d8dd77c 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -54,6 +54,9 @@ #include "systemlib.h" +// Didn't seem right to include up_internal.h, so direct extern instead. +extern void up_systemreset(void) noreturn_function; + void systemreset(bool to_bootloader) { diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index f8bff2f6f..235ab7186 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include @@ -228,8 +229,8 @@ do_show_print(void *arg, param_t param) if (!(arg == NULL)) { /* start search */ - char *ss = search_string; - char *pp = p_name; + const char *ss = search_string; + const char *pp = p_name; bool mismatch = false; /* XXX this comparison is only ok for trailing wildcards */ -- cgit v1.2.3 From bd5d3ebf70dc9e1aef106b60a840c17824d35b9b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 16:08:37 +0200 Subject: telemetry_statur: use 4 separate topics --- src/modules/commander/commander.cpp | 87 +++++++++++++++++++----------- src/modules/mavlink/mavlink_receiver.cpp | 86 +++++++++++++++-------------- src/modules/sdlog2/sdlog2.c | 31 ++++++----- src/modules/sdlog2/sdlog2_format.h | 8 +++ src/modules/sdlog2/sdlog2_messages.h | 35 +++++++----- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/telemetry_status.h | 14 ++++- 7 files changed, 166 insertions(+), 100 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 699ced1ab..0003ec106 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -767,7 +767,6 @@ int commander_thread_main(int argc, char *argv[]) hrt_abstime last_idle_time = 0; hrt_abstime start_time = 0; - hrt_abstime latest_heartbeat = 0; bool status_changed = true; bool param_init_forced = true; @@ -797,10 +796,16 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); - /* Subscribe to telemetry status */ - int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); - struct telemetry_status_s telemetry; - memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to telemetry status topics */ + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + telemetry_last_heartbeat[i] = 0; + telemetry_lost[i] = true; + } /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -882,7 +887,6 @@ int commander_thread_main(int argc, char *argv[]) bool arming_state_changed = false; bool main_state_changed = false; bool failsafe_old = false; - bool system_checked = false; while (!thread_should_exit) { @@ -939,15 +943,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } - /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && - (telemetry.heartbeat_time > 0) && - (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { - - (void)rc_calibration_check(mavlink_fd); - system_checked = true; - } - orb_check(sp_man_sub, &updated); if (updated) { @@ -960,10 +955,26 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(telemetry_sub, &updated); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + orb_check(telemetry_subs[i], &updated); - if (updated) { - orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + if (updated) { + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + + orb_copy(telemetry_status_orb_id[i], telemetry_subs[i], &telemetry); + + /* perform system checks when new telemetry link connected */ + if (mavlink_fd && + telemetry_last_heartbeat[i] == 0 && + telemetry.heartbeat_time > 0 && + hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + + (void)rc_calibration_check(mavlink_fd); + } + + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + } } orb_check(sensor_sub, &updated); @@ -1367,28 +1378,40 @@ int commander_thread_main(int argc, char *argv[]) } } - /* data link check */ - if (telemetry.heartbeat_time >= latest_heartbeat) { - if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* data links check */ + bool have_link = false; + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ - if (status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: data link regained"); - status.data_link_lost = false; - status_changed = true; + if (telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + telemetry_lost[i] = false; } - - /* Only consider data link with most recent heartbeat */ - latest_heartbeat = telemetry.heartbeat_time; + have_link = true; } else { - if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); - status.data_link_lost = true; - status_changed = true; + if (!telemetry_lost[i]) { + mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + telemetry_lost[i] = true; } } } + if (have_link) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + status.data_link_lost = false; + status_changed = true; + } + + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + status.data_link_lost = true; + status_changed = true; + } + } + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6d361052c..60da9c47d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -421,32 +421,35 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { - mavlink_radio_status_t rstatus; - mavlink_msg_radio_status_decode(msg, &rstatus); - - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); - - tstatus.timestamp = hrt_absolute_time(); - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; - tstatus.rssi = rstatus.rssi; - tstatus.remote_rssi = rstatus.remrssi; - tstatus.txbuf = rstatus.txbuf; - tstatus.noise = rstatus.noise; - tstatus.remote_noise = rstatus.remnoise; - tstatus.rxerrors = rstatus.rxerrors; - tstatus.fixed = rstatus.fixed; - - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_radio_status_t rstatus; + mavlink_msg_radio_status_decode(msg, &rstatus); + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; + tstatus.rssi = rstatus.rssi; + tstatus.remote_rssi = rstatus.remrssi; + tstatus.txbuf = rstatus.txbuf; + tstatus.noise = rstatus.noise; + tstatus.remote_noise = rstatus.remnoise; + tstatus.rxerrors = rstatus.rxerrors; + tstatus.fixed = rstatus.fixed; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; + } } void @@ -475,28 +478,31 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) void MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) { - mavlink_heartbeat_t hb; - mavlink_msg_heartbeat_decode(msg, &hb); + /* telemetry status supported only on first TELEMETRY_STATUS_ORB_ID_NUM mavlink channels */ + if (_mavlink->get_channel() < TELEMETRY_STATUS_ORB_ID_NUM) { + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); - /* ignore own heartbeats, accept only heartbeats from GCS */ - if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { - _telemetry_heartbeat_time = hrt_absolute_time(); + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { - struct telemetry_status_s tstatus; - memset(&tstatus, 0, sizeof(tstatus)); + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); - tstatus.timestamp = _telemetry_heartbeat_time; - tstatus.heartbeat_time = _telemetry_heartbeat_time; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - } else { - orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); + } } } } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 39e5b6c41..0d36fa2c6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -979,7 +979,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GVSP_s log_GVSP; struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; - struct log_TELE_s log_TELE; + struct log_TEL_s log_TEL; struct log_EST0_s log_EST0; struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; @@ -1019,7 +1019,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int esc_sub; int global_vel_sp_sub; int battery_sub; - int telemetry_sub; + int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; int range_finder_sub; int estimator_status_sub; int tecs_status_sub; @@ -1049,7 +1049,9 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.esc_sub = orb_subscribe(ORB_ID(esc_status)); subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); subs.battery_sub = orb_subscribe(ORB_ID(battery_status)); - subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + subs.telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); + } subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); @@ -1479,16 +1481,19 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- TELEMETRY --- */ - if (copy_if_updated(ORB_ID(telemetry_status), subs.telemetry_sub, &buf.telemetry)) { - log_msg.msg_type = LOG_TELE_MSG; - log_msg.body.log_TELE.rssi = buf.telemetry.rssi; - log_msg.body.log_TELE.remote_rssi = buf.telemetry.remote_rssi; - log_msg.body.log_TELE.noise = buf.telemetry.noise; - log_msg.body.log_TELE.remote_noise = buf.telemetry.remote_noise; - log_msg.body.log_TELE.rxerrors = buf.telemetry.rxerrors; - log_msg.body.log_TELE.fixed = buf.telemetry.fixed; - log_msg.body.log_TELE.txbuf = buf.telemetry.txbuf; - LOGBUFFER_WRITE_AND_COUNT(TELE); + for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { + if (copy_if_updated(telemetry_status_orb_id[i], subs.telemetry_subs[i], &buf.telemetry)) { + log_msg.msg_type = LOG_TEL0_MSG + i; + log_msg.body.log_TEL.rssi = buf.telemetry.rssi; + log_msg.body.log_TEL.remote_rssi = buf.telemetry.remote_rssi; + log_msg.body.log_TEL.noise = buf.telemetry.noise; + log_msg.body.log_TEL.remote_noise = buf.telemetry.remote_noise; + log_msg.body.log_TEL.rxerrors = buf.telemetry.rxerrors; + log_msg.body.log_TEL.fixed = buf.telemetry.fixed; + log_msg.body.log_TEL.txbuf = buf.telemetry.txbuf; + log_msg.body.log_TEL.heartbeat_time = buf.telemetry.heartbeat_time; + LOGBUFFER_WRITE_AND_COUNT(TEL); + } } /* --- BOTTOM DISTANCE --- */ diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h index dc5e6c8bd..aff0e3f48 100644 --- a/src/modules/sdlog2/sdlog2_format.h +++ b/src/modules/sdlog2/sdlog2_format.h @@ -91,6 +91,14 @@ struct log_format_s { .labels = _labels \ } +#define LOG_FORMAT_S(_name, _struct_name, _format, _labels) { \ + .type = LOG_##_name##_MSG, \ + .length = sizeof(struct log_##_struct_name##_s) + LOG_PACKET_HEADER_LEN, \ + .name = #_name, \ + .format = _format, \ + .labels = _labels \ + } + #define LOG_FORMAT_MSG 0x80 #define LOG_PACKET_SIZE(_name) LOG_PACKET_HEADER_LEN + sizeof(struct log_##_name##_s) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 8c05e87c5..08a87e179 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -276,18 +276,7 @@ struct log_DIST_s { uint8_t flags; }; -/* --- TELE - TELEMETRY STATUS --- */ -#define LOG_TELE_MSG 22 -struct log_TELE_s { - uint8_t rssi; - uint8_t remote_rssi; - uint8_t noise; - uint8_t remote_noise; - uint16_t rxerrors; - uint16_t fixed; - uint8_t txbuf; -}; - +// ID 22 available // ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ @@ -385,6 +374,23 @@ struct log_EST1_s { float s[16]; }; +/* --- TEL0..3 - TELEMETRY STATUS --- */ +#define LOG_TEL0_MSG 34 +#define LOG_TEL1_MSG 35 +#define LOG_TEL2_MSG 36 +#define LOG_TEL3_MSG 37 +struct log_TEL_s { + uint8_t rssi; + uint8_t remote_rssi; + uint8_t noise; + uint8_t remote_noise; + uint16_t rxerrors; + uint16_t fixed; + uint8_t txbuf; + uint64_t heartbeat_time; +}; + + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -432,7 +438,10 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), - LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), + LOG_FORMAT_S(TEL0, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL1, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL2, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), + LOG_FORMAT_S(TEL3, TEL, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf,HbTime"), LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 687fc1d4a..9b118205e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -186,7 +186,10 @@ ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/telemetry_status.h" -ORB_DEFINE(telemetry_status, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_2, struct telemetry_status_s); +ORB_DEFINE(telemetry_status_3, struct telemetry_status_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index e9e00d76c..c4b99d520 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -72,6 +72,18 @@ struct telemetry_status_s { * @} */ -ORB_DECLARE(telemetry_status); +ORB_DECLARE(telemetry_status_0); +ORB_DECLARE(telemetry_status_1); +ORB_DECLARE(telemetry_status_2); +ORB_DECLARE(telemetry_status_3); + +#define TELEMETRY_STATUS_ORB_ID_NUM 4 + +static const struct orb_metadata *telemetry_status_orb_id[TELEMETRY_STATUS_ORB_ID_NUM] = { + ORB_ID(telemetry_status_0), + ORB_ID(telemetry_status_1), + ORB_ID(telemetry_status_2), + ORB_ID(telemetry_status_3), +}; #endif /* TOPIC_TELEMETRY_STATUS_H */ -- cgit v1.2.3 From 06f08ad04d4cb21047944f350e4a75e88914e1e1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 6 Jul 2014 21:37:26 +0200 Subject: commander: require home position for MISSION, fallback to LOITER --- src/modules/commander/commander.cpp | 7 +++++++ src/modules/commander/state_machine_helper.cpp | 2 +- 2 files changed, 8 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index efa26eb97..1da364aa5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1718,6 +1718,13 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } print_reject_mode(status, "AUTO_MISSION"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } } // fallback to POSCTL diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 423ce2f23..7fb2e08db 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -271,7 +271,6 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; - case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { @@ -279,6 +278,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + case MAIN_STATE_AUTO_MISSION: case MAIN_STATE_AUTO_RTL: /* need global position and home position */ if (status->condition_global_position_valid && status->condition_home_position_valid) { -- cgit v1.2.3 From 2a79689224a381cf777c555d330f0a2aca8e3d9a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 16:51:06 -0700 Subject: Fix unused variable warnings --- src/modules/commander/commander.cpp | 4 ++-- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 +------ src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 2 +- src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/systemcmds/tests/test_mathlib.cpp | 3 +++ 5 files changed, 8 insertions(+), 10 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f83c1caf8..3d4a297f7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -773,7 +773,7 @@ int commander_thread_main(int argc, char *argv[]) bool updated = false; - bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -934,7 +934,7 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; /* re-check RC calibration */ - rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd)); + rc_calibration_check(mavlink_fd); } /* navigation parameters */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3e835cf81..3d6c62434 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -807,12 +807,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float eas2tas = 1.0f; // XXX calculate actual number based on current measurements - // XXX re-visit - float baro_altitude = _global_pos.alt; - /* filter speed and altitude for controller */ math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); - math::Vector<3> accel_earth = _R_nb * accel_body; float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; @@ -945,8 +941,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; - /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + /* Calculate altitude of last ordinary waypoint L */ float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index fc0a2551c..2e37d166e 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -220,7 +220,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector * is running) */ - bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); /* Write part of the status message */ _status.flightPathAngleSp = flightPathAngleSp; diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ca846a465..55a472bf6 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -346,7 +346,7 @@ MavlinkFTP::_workWrite(Request *req) MavlinkFTP::ErrorCode MavlinkFTP::_workRemove(Request *req) { - auto hdr = req->header(); + //auto hdr = req->header(); // for now, send error reply return kErrPerm; diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 00c649c75..df3ddb966 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -96,8 +96,11 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Vector<3> %% Vector<3>", v1 % v2); TEST_OP("Vector<3> length", v1.length()); TEST_OP("Vector<3> length squared", v1.length_squared()); +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-variable" TEST_OP("Vector<3> element read", volatile float a = v1(0)); TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]); +#pragma GCC diagnostic pop TEST_OP("Vector<3> element write", v1(0) = 1.0f); TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f); } -- cgit v1.2.3 From 6f1292838960ca9eb72dc1818250eb50f70ddb98 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Mon, 7 Jul 2014 20:00:44 -0700 Subject: Fix mainStateTransitionTest --- .../commander_tests/state_machine_helper_test.cpp | 213 ++++++++++++++------- 1 file changed, 147 insertions(+), 66 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index ee0d08391..2e18c4284 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -183,12 +183,12 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) // Safety switch arming tests - { "transition: init to standby, no safety switch", + { "transition: standby to armed, no safety switch", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_NOT_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, - { "transition: init to standby, safety switch off", + { "transition: standby to armed, safety switch off", { ARMING_STATE_STANDBY, ATT_DISARMED, ATT_READY_TO_ARM }, HIL_STATE_OFF, ATT_SENSORS_INITIALIZED, ATT_SAFETY_AVAILABLE, ATT_SAFETY_OFF, ARMING_STATE_ARMED, { ARMING_STATE_ARMED, ATT_ARMED, ATT_READY_TO_ARM }, TRANSITION_CHANGED }, @@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) armed.ready_to_arm = test->current_state.ready_to_arm; // Attempt transition - transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed); + transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */); // Validate result of transition ut_assert(test->assertMsg, test->expected_transition_result == result); @@ -300,70 +300,151 @@ bool StateMachineHelperTest::armingStateTransitionTest(void) bool StateMachineHelperTest::mainStateTransitionTest(void) { - struct vehicle_status_s current_state; - main_state_t new_main_state; + // This structure represent a single test case for testing Main State transitions. + typedef struct { + const char* assertMsg; // Text to show when test case fails + uint8_t condition_bits; // Bits for various condition_* values + main_state_t from_state; // State prior to transition request + main_state_t to_state; // State to transition to + transition_result_t expected_transition_result; // Expected result from main_state_transition call + } MainTransitionTest_t; + + // Bits for condition_bits + #define MTT_ALL_NOT_VALID 0 + #define MTT_ROTARY_WING 1 << 0 + #define MTT_LOC_ALT_VALID 1 << 1 + #define MTT_LOC_POS_VALID 1 << 2 + #define MTT_HOME_POS_VALID 1 << 3 + #define MTT_GLOBAL_POS_VALID 1 << 4 + + static const MainTransitionTest_t rgMainTransitionTests[] = { + + // TRANSITION_NOT_CHANGED tests + + { "no transition: identical states", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_MANUAL, TRANSITION_NOT_CHANGED }, + + // TRANSITION_CHANGED tests + + { "transition: MANUAL to ACRO", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ACRO, TRANSITION_CHANGED }, + + { "transition: ACRO to MANUAL", + MTT_ALL_NOT_VALID, + MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_MISSION - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED }, + + { "transition: AUTO_MISSION to MANUAL - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_LOITER - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_CHANGED }, + + { "transition: AUTO_LOITER to MANUAL - global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_AUTO_LOITER, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to AUTO_RTL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_CHANGED }, + + { "transition: AUTO_RTL to MANUAL - global position valid, home position valid", + MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID, + MAIN_STATE_AUTO_RTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - not rotary", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude valid", + MTT_ROTARY_WING | MTT_LOC_ALT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to ALTCTL - rotary, global position valid, local altitude not valid", + MTT_ROTARY_WING | MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_CHANGED }, + + { "transition: ALTCTL to MANUAL - local altitude valid", + MTT_LOC_ALT_VALID, + MAIN_STATE_ALTCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position not valid, global position valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: MANUAL to POSCTL - local position valid, global position not valid", + MTT_LOC_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_CHANGED }, + + { "transition: POSCTL to MANUAL - local position valid, global position valid", + MTT_LOC_POS_VALID, + MAIN_STATE_POSCTL, MAIN_STATE_MANUAL, TRANSITION_CHANGED }, + + // TRANSITION_DENIED tests + + { "no transition: MANUAL to AUTO_MISSION - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_LOITER - global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_LOITER, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position not valid, home position valid", + MTT_HOME_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to AUTO_RTL - global position valid, home position not valid", + MTT_GLOBAL_POS_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_AUTO_RTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to ALTCTL - rotary, global position not valid, local altitude not valid", + MTT_ROTARY_WING, + MAIN_STATE_MANUAL, MAIN_STATE_ALTCTL, TRANSITION_DENIED }, + + { "no transition: MANUAL to POSCTL - local position not valid, global position not valid", + MTT_ALL_NOT_VALID, + MAIN_STATE_MANUAL, MAIN_STATE_POSCTL, TRANSITION_DENIED }, + }; - // Identical states. - current_state.main_state = MAIN_STATE_MANUAL; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("no transition: identical states", - TRANSITION_NOT_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // AUTO to MANUAL. - current_state.main_state = MAIN_STATE_AUTO; - new_main_state = MAIN_STATE_MANUAL; - ut_assert("transition changed: auto to manual", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to ALTCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("tranisition: manual to altctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state); - - // MANUAL to ALTCTRL, invalid local altitude. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTCTL; - ut_assert("no transition: invalid local altitude", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to POSCTRL. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("transition: manual to posctrl", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state); - - // MANUAL to POSCTRL, invalid local position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSCTL; - ut_assert("no transition: invalid position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - - // MANUAL to AUTO. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = true; - new_main_state = MAIN_STATE_AUTO; - ut_assert("transition: manual to auto", - TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: auto", MAIN_STATE_AUTO == current_state.main_state); - - // MANUAL to AUTO, invalid global position. - current_state.main_state = MAIN_STATE_MANUAL; - current_state.condition_global_position_valid = false; - new_main_state = MAIN_STATE_AUTO; - ut_assert("no transition: invalid global position", - TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); + size_t cMainTransitionTests = sizeof(rgMainTransitionTests) / sizeof(rgMainTransitionTests[0]); + for (size_t i=0; ifrom_state; + current_state.is_rotary_wing = test->condition_bits & MTT_ROTARY_WING; + current_state.condition_local_altitude_valid = test->condition_bits & MTT_LOC_ALT_VALID; + current_state.condition_local_position_valid = test->condition_bits & MTT_LOC_POS_VALID; + current_state.condition_home_position_valid = test->condition_bits & MTT_HOME_POS_VALID; + current_state.condition_global_position_valid = test->condition_bits & MTT_GLOBAL_POS_VALID; + + // Attempt transition + transition_result_t result = main_state_transition(¤t_state, test->to_state); + + // Validate result of transition + ut_assert(test->assertMsg, test->expected_transition_result == result); + if (test->expected_transition_result == result) { + if (test->expected_transition_result == TRANSITION_CHANGED) { + ut_assert(test->assertMsg, test->to_state == current_state.main_state); + } else { + ut_assert(test->assertMsg, test->from_state == current_state.main_state); + } + } + } + return true; } -- cgit v1.2.3 From 3157b06fee99b57fe336f7772b293f8689ff8cdf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 15:52:19 +0200 Subject: Generalized the airspeed check --- src/modules/commander/state_machine_helper.cpp | 29 ++++++++++---------------- 1 file changed, 11 insertions(+), 18 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..d894c9db0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -666,29 +667,21 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || + hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; goto system_eval; } - struct differential_pressure_s diff_pres; - - ret = read(fd, &diff_pres, sizeof(diff_pres)); - - if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); - // XXX do not make this fatal yet - ret = OK; - } - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); - /* this is frickin' fatal */ - ret = ERROR; - goto system_eval; + if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; } } -- cgit v1.2.3 From 687612dd654bcdfb390bfdddf8a004b10119c2ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 10 Jul 2014 18:34:33 +0200 Subject: Do not abort if the sensor reset failed, only abort on no data --- src/modules/commander/airspeed_calibration.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 5d21d89d0..923c1a3ff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -95,10 +95,8 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to set scale / offsets for airspeed"); - mavlink_log_critical(mavlink_fd, "dpress reset failed"); - mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); - return ERROR; + warn("FAILED to reset - assuming analog"); + mavlink_log_critical(mavlink_fd, "assuming analog sensor"); } while (calibration_counter < calibration_count) { -- cgit v1.2.3 From 25f3f6e7f2dd87a831d25a9348a67ed918407d96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:12:31 +0200 Subject: airspeed calibration improvements for analog sensors --- src/modules/commander/airspeed_calibration.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 923c1a3ff..12e527a68 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -76,7 +76,7 @@ int do_airspeed_calibration(int mavlink_fd) /* Reset sensor parameters */ struct airspeed_scale airscale = { - 0.0f, + diff_pres_offset, 1.0f, }; @@ -97,6 +97,12 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } } while (calibration_counter < calibration_count) { -- cgit v1.2.3 From 4c13c67504fb2b66c97ac6b902e67a864f1173e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:02:22 +0200 Subject: Hotfix: Close fd before reusing it again --- src/modules/commander/state_machine_helper.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..ca95f139e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -666,6 +666,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ + close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd < 0) { -- cgit v1.2.3 From b97c867420f477f2c3a7dbc073975f5d872194cb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:51:13 +0200 Subject: Add a check command and fix error reporting --- src/modules/commander/commander.cpp | 19 ++++++++++++------- src/modules/commander/state_machine_helper.cpp | 23 ++++++++++------------- src/modules/commander/state_machine_helper.h | 2 ++ 3 files changed, 24 insertions(+), 20 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2058cd92e..106f05f57 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -286,15 +286,20 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running"); - print_status(); + /* commands needing the app to run below */ + if (!thread_running) { + warnx("\tcommander not started"); + exit(1); + } - } else { - warnx("\tcommander not started"); - } + if (!strcmp(argv[1], "status")) { + print_status(); + exit(0); + } + if (!strcmp(argv[1], "check")) { + int checkres = prearm_check(&status, mavlink_fd); + warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ca95f139e..d6b4d43ad 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,8 +70,6 @@ #endif static const int ERROR = -1; -static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { int ret; + bool failed = false; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret != OK) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + failed = true; goto system_eval; } @@ -646,22 +646,20 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) /* evaluate values */ float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.78f || accel_scale > 9.83f) { + if (accel_scale < 9.75f || accel_scale > 9.85f) { mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); } if (accel_scale > 30.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; - } else { - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } @@ -672,7 +670,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -681,20 +679,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = read(fd, &diff_pres, sizeof(diff_pres)); if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } } system_eval: close(fd); - return ret; + return (!failed); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 11072403e..ddc5bf154 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + #endif /* STATE_MACHINE_HELPER_H_ */ -- cgit v1.2.3 From a82d4fbb115e949a6a5c12d46308df1c20abfd99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 14:55:46 +0200 Subject: Hotfix: fix typo --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 106f05f57..6138a4c37 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -308,7 +308,7 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "2")) { + if (!strcmp(argv[1], "disarm")) { arm_disarm(false, mavlink_fd, "command line"); exit(0); } -- cgit v1.2.3 From 79c5d434bd5911d11e6968e0a339b40a6f82e033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:10:11 +0200 Subject: Commander: More hotfixes to prearm check routine --- src/modules/commander/commander.cpp | 5 ++++- src/modules/commander/state_machine_helper.cpp | 5 +++-- 2 files changed, 7 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6138a4c37..18761665a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -298,7 +298,9 @@ int commander_main(int argc, char *argv[]) } if (!strcmp(argv[1], "check")) { - int checkres = prearm_check(&status, mavlink_fd); + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int checkres = prearm_check(&status, mavlink_fd_local); + close(mavlink_fd_local); warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } @@ -329,6 +331,7 @@ void usage(const char *reason) void print_status() { + warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d6b4d43ad..5cbc95920 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -664,11 +664,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { + /* accel done, close it */ close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + if (fd <= 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; @@ -693,5 +694,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) system_eval: close(fd); - return (!failed); + return (failed); } -- cgit v1.2.3 From a118e8dbdd72e23a47841577dfcd7a45007eaedc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 15:17:21 +0200 Subject: Make commander error message more verbose --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5cbc95920..ce1bd9ada 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -647,7 +647,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); + mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); } if (accel_scale > 30.0f /* m/s^2 */) { -- cgit v1.2.3 From 11d9724563f005ed722d326e8bef125bfec29865 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 16:03:27 +0200 Subject: Stop warning users just because they shake a bit, be more strict to catch sensor failures or calibration errors in time --- src/modules/commander/state_machine_helper.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ce1bd9ada..363f5e8eb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -644,14 +644,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(acc)) { /* evaluate values */ - float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale < 9.75f || accel_scale > 9.85f) { - mavlink_log_info(mavlink_fd, "#audio: Accel calib. recommended (%8.4f).", (double)accel_scale); - } - - if (accel_scale > 30.0f /* m/s^2 */) { + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; -- cgit v1.2.3 From 899657613e44f0d4bbdb66644b35420d7381ff99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 22:09:25 +0200 Subject: airspeed cal: Improve user feedback --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 12e527a68..7599fc777 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -96,7 +96,7 @@ int do_airspeed_calibration(int mavlink_fd) if (!paramreset_successful) { warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "assuming analog sensor"); + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); -- cgit v1.2.3 From 45617e9f4385e80846c571b237e220216192d6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 16:09:14 +0200 Subject: Airspeed calibration improvement, enforce correct pitot order --- src/modules/commander/airspeed_calibration.cpp | 137 ++++++++++++++++++++----- src/modules/ekf_att_pos_estimator/InertialNav | 1 + 2 files changed, 112 insertions(+), 26 deletions(-) create mode 160000 src/modules/ekf_att_pos_estimator/InertialNav (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 7599fc777..a1a20abc9 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -64,14 +64,12 @@ int do_airspeed_calibration(int mavlink_fd) { /* give directions */ mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - mavlink_log_info(mavlink_fd, "ensure airspeed sensor is not registering wind"); - const int calibration_count = 2000; + const unsigned calibration_count = 2000; int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; - int calibration_counter = 0; float diff_pres_offset = 0.0f; /* Reset sensor parameters */ @@ -105,29 +103,58 @@ int do_airspeed_calibration(int mavlink_fd) } } - while (calibration_counter < calibration_count) { + for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + unsigned calibration_counter = 0; - int poll_ret = poll(fds, 1, 1000); + if (i == 0) { + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); + } - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + float diff_pres_offset = 0.0f; - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count); - } + while (calibration_counter < calibration_count) { - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); - close(diff_pres_sub); - return ERROR; + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + if (i > 0) { + + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + return ERROR; + } + } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } } } @@ -151,14 +178,72 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - tune_neutral(true); - close(diff_pres_sub); - return OK; - } else { mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); close(diff_pres_sub); return ERROR; } + + /* wait 500 ms to ensure parameter propagated through the system */ + usleep(500 * 1000); + + calibration_counter = 0; + diff_pres_offset = 0.0f; + + /* just take a few samples and make sure pitot tubes are not reversed */ + while (calibration_counter < 50) { + + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + float curr_avg = (diff_pres_offset / calibration_counter); + + if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } + + /* do not log negative values in the second go */ + if (curr_avg < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + close(diff_pres_sub); + + /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ + + diff_pres_offset = 0.0f; + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + close(diff_pres_sub); + return ERROR; + } + + /* save */ + (void)param_save_default(); + + return ERROR; + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + } + + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); + tune_neutral(true); + close(diff_pres_sub); + return OK; } diff --git a/src/modules/ekf_att_pos_estimator/InertialNav b/src/modules/ekf_att_pos_estimator/InertialNav new file mode 160000 index 000000000..8b65d755b --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/InertialNav @@ -0,0 +1 @@ +Subproject commit 8b65d755b8c4834f90a323172c25d91c45068e21 -- cgit v1.2.3 From 0d1ac4235411e8f05f96bcbe51558d92f0d86cf6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 19:10:08 +0200 Subject: airspeed calibration: Update and resolve compile errors --- src/modules/commander/airspeed_calibration.cpp | 75 ++++++++++++-------------- 1 file changed, 33 insertions(+), 42 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1a20abc9..421e53ae0 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -51,6 +51,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -103,58 +104,48 @@ int do_airspeed_calibration(int mavlink_fd) } } - for (unsigned i = 0; i < (sizeof(diff_pres_offset) / sizeof(diff_pres_offset[0])); i++) { + unsigned calibration_counter = 0; - unsigned calibration_counter = 0; - - if (i == 0) { - mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); - usleep(500 * 1000); - } - - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + mavlink_log_critical(mavlink_fd, "Ensure sensor is not measuring wind"); + usleep(500 * 1000); - if (i > 0) { + while (calibration_counter < calibration_count) { - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } + /* wait blocking for new data */ + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - } + int poll_ret = poll(fds, 1, 1000); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); - } + if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + usleep(5000 * 1000); + continue; + } - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + /* do not log negative values in the second go */ + if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); return ERROR; } + + diff_pres_offset += diff_pres.differential_pressure_raw_pa; + calibration_counter++; + + if (calibration_counter % (calibration_count / 20) == 0) { + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + } + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; } } -- cgit v1.2.3 From 7bf0e6f3e26ee9e10073293677563f6862758557 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 21:47:26 +0200 Subject: Better airspeed feedback --- src/modules/commander/airspeed_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 421e53ae0..a1b8816ff 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,13 +200,13 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < calc_indicated_airspeed(-5.0f)) { + if (curr_avg < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); -- cgit v1.2.3 From 67d23253c3b3ec04bfd9d5f7e09d3d405ceba7c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:00:50 +0200 Subject: airspeed cal build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a1b8816ff..529a5b32f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -200,7 +200,7 @@ int do_airspeed_calibration(int mavlink_fd) float curr_avg = (diff_pres_offset / calibration_counter); if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", curr_avg); + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } -- cgit v1.2.3 From 4824a4e8ac93d992e1401f768291cca65ba46acc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:18:59 +0200 Subject: Fix for dynamic / static part of calibration --- src/modules/commander/airspeed_calibration.cpp | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 529a5b32f..01db34be2 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -121,19 +121,6 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (calc_indicated_airspeed(fabsf(diff_pres.differential_pressure_raw_pa)) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot"); - usleep(5000 * 1000); - continue; - } - - /* do not log negative values in the second go */ - if ((diff_pres_offset / calibration_counter) < calc_indicated_airspeed(-5.0f)) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); - close(diff_pres_sub); - return ERROR; - } - diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; @@ -197,16 +184,16 @@ int do_airspeed_calibration(int mavlink_fd) diff_pres_offset += diff_pres.differential_pressure_raw_pa; calibration_counter++; - float curr_avg = (diff_pres_offset / calibration_counter); + float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - if (calc_indicated_airspeed(fabsf(curr_avg)) < 10.0f) { + if (fabsf(curr_avg) < 10.0f) { mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); usleep(5000 * 1000); continue; } /* do not log negative values in the second go */ - if (curr_avg < -calc_indicated_airspeed(5.0f)) { + if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); -- cgit v1.2.3 From 34abf5c40cda538f204d313cc49715b5a938d168 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 22:50:56 +0200 Subject: airspeed cal: Fix up logic --- src/modules/commander/airspeed_calibration.cpp | 25 +++++++++++-------------- 1 file changed, 11 insertions(+), 14 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 01db34be2..47c9c7af5 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -165,11 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); - calibration_counter = 0; - diff_pres_offset = 0.0f; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 50) { + while (calibration_counter < 10) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,19 +178,15 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_raw_pa; - calibration_counter++; - - float curr_avg_airspeed = calc_indicated_airspeed(diff_pres_offset / calibration_counter); - - if (fabsf(curr_avg) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f m/s)", (double)curr_avg); - usleep(5000 * 1000); + if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { + mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + usleep(3000 * 1000); continue; } - /* do not log negative values in the second go */ - if (curr_avg_airspeed < -calc_indicated_airspeed(5.0f)) { + /* do not allow negative values */ + if (diff_pres.differential_pressure_raw_pa < 0.0f) { mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); close(diff_pres_sub); @@ -209,7 +202,11 @@ int do_airspeed_calibration(int mavlink_fd) /* save */ (void)param_save_default(); + close(diff_pres_sub); return ERROR; + } else { + close(diff_pres_sub); + return OK; } } else if (poll_ret == 0) { -- cgit v1.2.3 From 65409ad2c8ba10d95cbcfa088951abc40eb46774 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 12 Jul 2014 23:30:00 +0200 Subject: Airspeed calibration improvements --- src/modules/commander/airspeed_calibration.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 47c9c7af5..d52cafb8f 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -125,7 +125,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { - mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count / 2); + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 80) / calibration_count); } } else if (poll_ret == 0) { @@ -165,6 +165,8 @@ int do_airspeed_calibration(int mavlink_fd) /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + calibration_counter = 0; + /* just take a few samples and make sure pitot tubes are not reversed */ while (calibration_counter < 10) { @@ -178,16 +180,18 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - if (fabsf(diff_pres.differential_pressure_raw_pa) < 10.0f) { - mavlink_log_critical(mavlink_fd, "Put a finger on front hole of pitot (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + + if (fabsf(calibrated_pa) < 9.0f) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", + (double)calibrated_pa); usleep(3000 * 1000); continue; } /* do not allow negative values */ - if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static<->dynamic ports,restart"); + if (calibrated_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -205,8 +209,9 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - close(diff_pres_sub); - return OK; + mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", + (double)diff_pres.differential_pressure_raw_pa); + break; } } else if (poll_ret == 0) { @@ -217,6 +222,8 @@ int do_airspeed_calibration(int mavlink_fd) } } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); + mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); tune_neutral(true); close(diff_pres_sub); -- cgit v1.2.3 From 716e20fab6d63f3dc6f80c70f382d6b0f45f9340 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:39:13 +0200 Subject: Fix accel cal docs. --- src/modules/commander/accelerometer_calibration.cpp | 3 +-- src/modules/commander/accelerometer_calibration.h | 3 +-- 2 files changed, 2 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index be465ab76..7a4e7a766 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 1cf9c0977..6b7e16d44 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 -- cgit v1.2.3 From 259014b0d50426b0fbed3b9eac5a1d34aaa5b211 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:40:09 +0200 Subject: Documentation only and unused defines only cleanup. --- src/modules/commander/airspeed_calibration.h | 2 +- src/modules/commander/baro_calibration.cpp | 2 +- src/modules/commander/baro_calibration.h | 2 +- src/modules/commander/calibration_messages.h | 3 +-- src/modules/commander/calibration_routines.cpp | 3 +-- src/modules/commander/calibration_routines.h | 3 +-- src/modules/commander/mag_calibration.h | 2 +- src/modules/commander/state_machine_helper.h | 10 ++++------ 8 files changed, 11 insertions(+), 16 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h index 71c701fc2..8c6b65ff1 100644 --- a/src/modules/commander/airspeed_calibration.h +++ b/src/modules/commander/airspeed_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 diff --git a/src/modules/commander/baro_calibration.cpp b/src/modules/commander/baro_calibration.cpp index 3123c4087..da98644b4 100644 --- a/src/modules/commander/baro_calibration.cpp +++ b/src/modules/commander/baro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h index bc42bc6cf..ce78ae700 100644 --- a/src/modules/commander/baro_calibration.h +++ b/src/modules/commander/baro_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * 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 diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index fd8b8564d..b1e209efc 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * 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 diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 43f341ae7..5796204bf 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index e3e7fbafd..3c8e49ee9 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2012 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 diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h index a101cd7b1..c14f948cc 100644 --- a/src/modules/commander/mag_calibration.h +++ b/src/modules/commander/mag_calibration.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 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 diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index ddc5bf154..bb1b87e71 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 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 @@ -36,14 +34,14 @@ /** * @file state_machine_helper.h * State machine helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef STATE_MACHINE_HELPER_H_ #define STATE_MACHINE_HELPER_H_ -#define GPS_NOFIX_COUNTER_LIMIT 4 //need GPS_NOFIX_COUNTER_LIMIT gps packets with a bad fix to call an error (if outdoor) -#define GPS_GOTFIX_COUNTER_REQUIRED 4 //need GPS_GOTFIX_COUNTER_REQUIRED gps packets with a good fix to obtain position lock - #include #include #include -- cgit v1.2.3 From ea11bc6c4bf7180ae4e321eebcaf817eebb18e6c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:41:39 +0200 Subject: Command handling: Fix up local variable usage and status prints. --- src/modules/commander/commander.cpp | 67 +++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 29 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e14dff197..009e0ac01 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -403,10 +403,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char return arming_res; } -bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) +bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, + struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, + struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ - if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components + if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } @@ -425,7 +427,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd); // Transition the arming state arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); @@ -434,43 +436,43 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_ret = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status_local, MAIN_STATE_ACRO); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { /* OFFBOARD */ - main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); + main_ret = main_state_transition(status_local, MAIN_STATE_OFFBOARD); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_ret = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_ret = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status_local, MAIN_STATE_MANUAL); } } } @@ -485,22 +487,25 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. - // We use an float epsilon delta to test float equality. - if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) { - mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", (double)cmd->param1); + // Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints + // for logic state parameters + + if (static_cast(cmd->param1 + 0.5f) != 0 && static_cast(cmd->param1 + 0.5f) != 1) { + mavlink_log_critical(mavlink_fd, "Unsupported ARM_DISARM param: %.3f", (double)cmd->param1); } else { + bool cmd_arms = (static_cast(cmd->param1 + 0.5f) == 1); + // Flick to inair restore first if this comes from an onboard system - if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) { - status->arming_state = ARMING_STATE_IN_AIR_RESTORE; + if (cmd->source_system == status_local->system_id && cmd->source_component == status_local->component_id) { + status_local->arming_state = ARMING_STATE_IN_AIR_RESTORE; } - transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command"); + transition_result_t arming_res = arm_disarm(cmd_arms, mavlink_fd, "arm/disarm component command"); if (arming_res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); + mavlink_log_critical(mavlink_fd, "REJECTING component arm cmd"); cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { @@ -512,20 +517,24 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_OVERRIDE_GOTO: { // TODO listen vehicle_command topic directly from navigator (?) - unsigned int mav_goto = cmd->param1; + + // Increase by 0.5f and rely on the integer cast + // implicit floor(). This is the *safest* way to + // convert from floats representing small ints to actual ints. + unsigned int mav_goto = (cmd->param1 + 0.5f); if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->nav_state = NAVIGATION_STATE_AUTO_LOITER; - mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_LOITER; + mavlink_log_critical(mavlink_fd, "Pause mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); + status_local->nav_state = NAVIGATION_STATE_AUTO_MISSION; + mavlink_log_critical(mavlink_fd, "Continue mission cmd"); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", + mavlink_log_critical(mavlink_fd, "REJ CMD: %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f", (double)cmd->param1, (double)cmd->param2, (double)cmd->param3, @@ -543,7 +552,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //XXX: to enable the parachute, a param needs to be set //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO - if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { + if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -561,7 +570,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (use_current) { /* use current position */ - if (status->condition_global_position_valid) { + if (status_local->condition_global_position_valid) { home->lat = global_pos->lat; home->lon = global_pos->lon; home->alt = global_pos->alt; @@ -598,7 +607,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } /* mark home position as set */ - status->condition_home_position_valid = true; + status_local->condition_home_position_valid = true; } } break; -- cgit v1.2.3 From 3ec9ffa66166b10101887cc4077d1b02d4a0897b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:42:02 +0200 Subject: Buzzer and led: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 009e0ac01..85c9ccbaa 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -697,11 +697,11 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds"); + warnx("ERROR: LED INIT FAIL"); } if (buzzer_init() != OK) { - warnx("ERROR: Failed to initialize buzzer"); + warnx("ERROR: BUZZER INIT FAIL"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); -- cgit v1.2.3 From 76f7960d77046c33a771be49b57e32c957e7a2ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:43:56 +0200 Subject: Mavlink text feedback: Remove now unneeded audio tag for critical messages, make overall printing more efficient. Remove buffers where no buffers are needed. --- src/modules/commander/commander.cpp | 55 +++++++++++++++++-------------------- 1 file changed, 25 insertions(+), 30 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 85c9ccbaa..76cfd5feb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,8 +775,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[cmd] dataman_id=%d, count=%u, current=%d", mission.dataman_id, mission.count, mission.current_seq); } else { - warnx("reading mission state failed"); - mavlink_log_info(mavlink_fd, "[cmd] reading mission state failed"); + const char *missionfail = "reading mission state failed"; + warnx("%s", missionfail); + mavlink_log_critical(mavlink_fd, missionfail); /* initialize mission state in dataman */ mission.dataman_id = 0; @@ -789,8 +790,6 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(offboard_mission), mission_pub, &mission); } - mavlink_log_info(mavlink_fd, "[cmd] started"); - int ret; pthread_attr_t commander_low_prio_attr; @@ -1083,7 +1082,7 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + mavlink_log_info(mavlink_fd, "DISARMED by safety switch"); arming_state_changed = true; } } @@ -1187,10 +1186,10 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; if (status.condition_landed) { - mavlink_log_critical(mavlink_fd, "#audio: LANDED"); + mavlink_log_critical(mavlink_fd, "LANDED MODE"); } else { - mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); + mavlink_log_critical(mavlink_fd, "IN AIR MODE"); } } } @@ -1270,14 +1269,14 @@ int commander_thread_main(int argc, char *argv[]) /* 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) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY"); + mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED"); status.battery_warning = VEHICLE_BATTERY_WARNING_LOW; status_changed = true; } 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 */ critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY"); + mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY"); status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { @@ -1339,12 +1338,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, "#audio: detected RC signal first time"); + mavlink_log_critical(mavlink_fd, "detected RC signal first time"); status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: RC signal regained"); + mavlink_log_critical(mavlink_fd, "RC signal regained"); status_changed = true; } } @@ -1385,7 +1384,7 @@ int commander_thread_main(int argc, char *argv[]) * the system can be armed in auto if armed via the GCS. */ if (status.main_state != MAIN_STATE_MANUAL) { - print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); + print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); if (arming_ret == TRANSITION_CHANGED) { @@ -1405,16 +1404,16 @@ int commander_thread_main(int argc, char *argv[]) if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { - mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + mavlink_log_info(mavlink_fd, "ARMED by RC"); } else { - mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + mavlink_log_info(mavlink_fd, "DISARMED by RC"); } arming_state_changed = true; } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); + mavlink_log_critical(mavlink_fd, "arming state transition denied"); tune_negative(true); } @@ -1428,12 +1427,12 @@ int commander_thread_main(int argc, char *argv[]) } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ - mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); + mavlink_log_critical(mavlink_fd, "main state transition denied"); } } else { if (!status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); + mavlink_log_critical(mavlink_fd, "RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } @@ -1445,14 +1444,14 @@ int commander_thread_main(int argc, char *argv[]) if (hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i regained", i); + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } have_link = true; } else { if (!telemetry_lost[i]) { - mavlink_log_critical(mavlink_fd, "#audio: data link %i lost", i); + mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; } } @@ -1467,7 +1466,7 @@ int commander_thread_main(int argc, char *argv[]) } else { if (!status.data_link_lost) { - mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: ALL DATA LINKS LOST"); + mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; status_changed = true; } @@ -2010,15 +2009,13 @@ set_control_mode() } void -print_reject_mode(struct vehicle_status_s *status, const char *msg) +print_reject_mode(struct vehicle_status_s *status_local, 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, "#audio: REJECT %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, "REJECT %s", msg); /* only buzz if armed, because else we're driving people nuts indoors they really need to look at the leds as well. */ @@ -2033,9 +2030,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, "#audio: %s", msg); - mavlink_log_critical(mavlink_fd, s); + mavlink_log_critical(mavlink_fd, msg); tune_negative(true); } } @@ -2048,12 +2043,12 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_DENIED: - mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command denied: %u", cmd.command); tune_negative(true); break; case VEHICLE_CMD_RESULT_FAILED: - mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command failed: %u", cmd.command); tune_negative(true); break; @@ -2064,7 +2059,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul break; case VEHICLE_CMD_RESULT_UNSUPPORTED: - mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command); + mavlink_log_critical(mavlink_fd, "command unsupported: %u", cmd.command); tune_negative(true); break; -- cgit v1.2.3 From f2b30be92ae946e8e40faade6cb10edfb037fdaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:44:27 +0200 Subject: commander status leds: Fix -Wshadow warning. --- src/modules/commander/commander.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 76cfd5feb..e5ea237fc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1650,22 +1650,22 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val } void -control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed) +control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actuator_armed, bool changed) { /* driving rgbled */ if (changed) { bool set_normal_color = false; /* set mode */ - if (status->arming_state == ARMING_STATE_ARMED) { + if (status_local->arming_state == ARMING_STATE_ARMED) { rgbled_set_mode(RGBLED_MODE_ON); set_normal_color = true; - } else if (status->arming_state == ARMING_STATE_ARMED_ERROR) { + } else if (status_local->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) { + } else if (status_local->arming_state == ARMING_STATE_STANDBY) { rgbled_set_mode(RGBLED_MODE_BREATHE); set_normal_color = true; @@ -1676,12 +1676,12 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { + if (status_local->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status_local->failsafe) { 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) { + if (status_local->condition_local_position_valid) { rgbled_set_color(RGBLED_COLOR_GREEN); } else { @@ -1714,7 +1714,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a #endif /* give system warnings on error LED, XXX maybe add memory usage warning too */ - if (status->load > 0.95f) { + if (status_local->load > 0.95f) { if (leds_counter % 2 == 0) { led_toggle(LED_AMBER); } -- cgit v1.2.3 From 66d5bc20c095a4dbd937b21c4f5fc1a67205f2d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:02 +0200 Subject: commander RC handling: Fix -Wshadow warnings. --- src/modules/commander/commander.cpp | 42 ++++++++++++++++++------------------- 1 file changed, 21 insertions(+), 21 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e5ea237fc..0c4d48dea 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1727,16 +1727,16 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } transition_result_t -set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man) +set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_setpoint_s *sp_man) { /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_OFFBOARD); + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { - print_reject_mode(status, "OFFBOARD"); + print_reject_mode(status_local, "OFFBOARD"); } else { return res; @@ -1751,78 +1751,78 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin case SWITCH_POS_OFF: // MANUAL if (sp_man->acro_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_ACRO); + res = main_state_transition(status_local, MAIN_STATE_ACRO); } else { - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); } // TRANSITION_DENIED is not possible here break; case SWITCH_POS_MIDDLE: // ASSIST if (sp_man->posctl_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "POSCTL"); + print_reject_mode(status_local, "POSCTL"); } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } if (sp_man->posctl_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTCTL"); + print_reject_mode(status_local, "ALTCTL"); } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case SWITCH_POS_ON: // AUTO if (sp_man->return_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_RTL); + res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_RTL"); + print_reject_mode(status_local, "AUTO_RTL"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } } else if (sp_man->loiter_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_LOITER"); + print_reject_mode(status_local, "AUTO_LOITER"); } else { - res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + res = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - print_reject_mode(status, "AUTO_MISSION"); + print_reject_mode(status_local, "AUTO_MISSION"); // fallback to LOITER if home position not set - res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1830,21 +1830,21 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } // fallback to POSCTL - res = main_state_transition(status, MAIN_STATE_POSCTL); + res = main_state_transition(status_local, MAIN_STATE_POSCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to ALTCTL - res = main_state_transition(status, MAIN_STATE_ALTCTL); + res = main_state_transition(status_local, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } // fallback to MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + res = main_state_transition(status_local, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; -- cgit v1.2.3 From afc8908d38ff2eaabc2ca35363ac04697152cb6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:45:32 +0200 Subject: commander: More docs-only changes in headers. --- src/modules/commander/commander_helper.cpp | 10 ++++++---- src/modules/commander/commander_helper.h | 9 ++++++--- 2 files changed, 12 insertions(+), 7 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 80e6861f6..d5fe122cb 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * Copyright (c) 2013, 2014 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 @@ -37,6 +34,11 @@ /** * @file commander_helper.cpp * Commander helper functions implementations + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * */ #include diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index e75f2592f..a49c9e263 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2013, 2014 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 @@ -36,6 +34,9 @@ /** * @file commander_helper.h * Commander helper functions definitions + * + * @author Thomas Gubler + * @author Julian Oes */ #ifndef COMMANDER_HELPER_H_ @@ -77,6 +78,8 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern); * Use integral of current if battery capacity known (BAT_CAPACITY parameter set), * else use simple estimate based on voltage. * + * @param voltage the current battery voltage + * @param discharged the discharged capacity * @return the estimated remaining capacity in 0..1 */ float battery_remaining_estimate_voltage(float voltage, float discharged); -- cgit v1.2.3 From 7525c290f2cdfd058d54e5e6eb2d2e522d070889 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 11:46:36 +0200 Subject: commander: more text feedback improvements. --- src/modules/commander/state_machine_helper.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 363f5e8eb..87a60e7fd 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -87,7 +87,7 @@ static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { }; // You can index into the array with an arming_state_t in order to get it's textual representation -static const char *state_names[ARMING_STATE_MAX] = { +static const char * const state_names[ARMING_STATE_MAX] = { "ARMING_STATE_INIT", "ARMING_STATE_STANDBY", "ARMING_STATE_ARMED", @@ -160,7 +160,7 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if we need safety switch press } else if (safety->safety_switch_available && !safety->safety_off) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch!"); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!"); valid_transition = false; } @@ -625,7 +625,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL SENSOR MISSING"); failed = true; goto system_eval; } @@ -633,7 +633,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL CALIBRATION"); failed = true; goto system_eval; } @@ -647,14 +647,14 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); - mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE"); + mavlink_log_critical(mavlink_fd, "hold still while arming"); /* this is frickin' fatal */ failed = true; goto system_eval; } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL READ"); /* this is frickin' fatal */ failed = true; goto system_eval; @@ -667,7 +667,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); if (fd <= 0) { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; } @@ -678,11 +678,11 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(diff_pres)) { if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + mavlink_log_critical(mavlink_fd, "ARM WARNING: AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet } } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); + mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED READ"); /* this is frickin' fatal */ failed = true; goto system_eval; -- cgit v1.2.3 From 0d51230d4e3b87dd67819e594895b24a0a8a0306 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:04:43 +0200 Subject: commander: Final MAVLink text feedback cleanup --- src/modules/commander/state_machine_helper.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87a60e7fd..56c59ad3d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -171,16 +171,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f)) { + if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f))) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); valid_transition = false; } } -- cgit v1.2.3 From 744eed91dca8a5f2ad8ead7ffa812d25755bcc93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:43:10 +0200 Subject: Fix calibration counter usage --- src/modules/commander/airspeed_calibration.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index d52cafb8f..a2f11b14a 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -168,7 +168,7 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 10) { + while (calibration_counter < 300) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -182,16 +182,19 @@ int do_airspeed_calibration(int mavlink_fd) float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; - if (fabsf(calibrated_pa) < 9.0f) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%.1f Pa, #h101)", - (double)calibrated_pa); - usleep(3000 * 1000); + if (fabsf(calibrated_pa) < 50.0f) { + if (calibration_counter % 20 == 0) { + mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + (int)calibrated_pa); + } + usleep(100 * 1000); + calibration_counter++; continue; } /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "Negative val: swap static vs dynamic ports,restart"); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -209,8 +212,8 @@ int do_airspeed_calibration(int mavlink_fd) close(diff_pres_sub); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: (%.1f Pa)", - (double)diff_pres.differential_pressure_raw_pa); + mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + (int)calibrated_pa); break; } -- cgit v1.2.3 From 8590d555b432f206e2b88893ea5198c1398aa99c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 12:44:57 +0200 Subject: Fix calibration counter usage, take every sample --- src/modules/commander/airspeed_calibration.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index a2f11b14a..10d04aae9 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -167,8 +167,8 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter = 0; - /* just take a few samples and make sure pitot tubes are not reversed */ - while (calibration_counter < 300) { + /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ + while (calibration_counter < 1500) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -181,14 +181,13 @@ int do_airspeed_calibration(int mavlink_fd) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; + calibration_counter++; if (fabsf(calibrated_pa) < 50.0f) { - if (calibration_counter % 20 == 0) { + if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", (int)calibrated_pa); } - usleep(100 * 1000); - calibration_counter++; continue; } -- cgit v1.2.3 From d4a867071a2183069d8902292357133856cd2ffd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 15:44:15 +0200 Subject: airspeed: Better calibration messages --- src/modules/commander/airspeed_calibration.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 10d04aae9..469f15632 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -193,7 +193,7 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static vs dynamic ports,restart", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -206,9 +206,12 @@ int do_airspeed_calibration(int mavlink_fd) } /* save */ + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 0); (void)param_save_default(); close(diff_pres_sub); + + mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", -- cgit v1.2.3 From 1dffa750d8b91af8f600ba4cf1fbcdcc61edf80f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:44:32 +0200 Subject: added detailed print --- src/modules/commander/airspeed_calibration.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 469f15632..bc12037c3 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,6 +162,8 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); -- cgit v1.2.3 From 56ad0c708d2695515489802676cc295f040b3606 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 13 Jul 2014 22:53:50 +0200 Subject: Fixed compile error --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index bc12037c3..2e3f89e65 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -162,7 +162,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)calibrated_pa); + mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); -- cgit v1.2.3 From 7968c6864e1255b4a65427187119aec2c3fc7ae0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:04:02 +0200 Subject: Force update of offset, do not add offset in final value --- src/modules/commander/airspeed_calibration.cpp | 21 +++++++++++++++------ src/modules/commander/state_machine_helper.cpp | 4 ++-- 2 files changed, 17 insertions(+), 8 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 2e3f89e65..ea81ac6a6 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -140,6 +140,16 @@ int do_airspeed_calibration(int mavlink_fd) if (isfinite(diff_pres_offset)) { + int fd_scale = open(AIRSPEED_DEVICE_PATH, 0); + airscale.offset_pa = diff_pres_offset; + if (fd_scale > 0) { + if (OK != ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { + mavlink_log_critical(mavlink_fd, "airspeed offset update failed"); + } + + close(fd_scale); + } + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); @@ -182,20 +192,19 @@ int do_airspeed_calibration(int mavlink_fd) if (poll_ret) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - float calibrated_pa = diff_pres.differential_pressure_raw_pa - diff_pres_offset; calibration_counter++; - if (fabsf(calibrated_pa) < 50.0f) { + if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ - if (calibrated_pa < 0.0f) { - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)calibrated_pa); + if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -217,7 +226,7 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } else { mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", - (int)calibrated_pa); + (int)diff_pres.differential_pressure_raw_pa); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 09ea12c38..372ba9d7d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -668,8 +668,8 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) struct airspeed_s airspeed; - if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) || - hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) { + if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) || + (hrt_elapsed_time(&airspeed.timestamp) > (50 * 1000))) { mavlink_log_critical(mavlink_fd, "ARM FAIL: AIRSPEED SENSOR MISSING"); failed = true; goto system_eval; -- cgit v1.2.3 From 180b83cc6dc6872c5248993b5409ac835a201114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 00:41:12 +0200 Subject: Better analog error handling --- src/modules/commander/airspeed_calibration.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index ea81ac6a6..92d50a025 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -94,9 +94,17 @@ int do_airspeed_calibration(int mavlink_fd) } if (!paramreset_successful) { - warn("FAILED to reset - assuming analog"); - mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + /* only warn if analog scaling is zero */ + float analog_scaling = 0.0f; + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + if (fabsf(analog_scaling) < 0.1f) { + mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); + close(diff_pres_sub); + return ERROR; + } + + /* set scaling offset parameter */ if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); close(diff_pres_sub); -- cgit v1.2.3 From ec8d395a2d270bd77e873f616a53ee0771c94165 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:07:30 +0200 Subject: build fix --- src/modules/commander/airspeed_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 92d50a025..423d88682 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -97,7 +97,7 @@ int do_airspeed_calibration(int mavlink_fd) /* only warn if analog scaling is zero */ float analog_scaling = 0.0f; - param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)) + param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); if (fabsf(analog_scaling) < 0.1f) { mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); close(diff_pres_sub); -- cgit v1.2.3 From 9ce7820e419d2ffa379fb7a3cc168f500623fa3d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:32:51 +0200 Subject: Make instructions in commander more obvious for airspeed calibration --- src/modules/commander/airspeed_calibration.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) (limited to 'src/modules/commander') diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 423d88682..598cfe9e2 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,15 +180,16 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Stored offset of %d Pa", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); calibration_counter = 0; + const int maxcount = 3000; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ - while (calibration_counter < 1500) { + while (calibration_counter < maxcount) { /* wait blocking for new data */ struct pollfd fds[1]; @@ -204,7 +205,7 @@ int do_airspeed_calibration(int mavlink_fd) if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Create airflow on pitot (%d Pa, #h101)", + mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", (int)diff_pres.differential_pressure_raw_pa); } continue; @@ -212,6 +213,8 @@ int do_airspeed_calibration(int mavlink_fd) /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { + mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); @@ -246,6 +249,12 @@ int do_airspeed_calibration(int mavlink_fd) } } + if (calibration_counter == maxcount) { + mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); + close(diff_pres_sub); + return ERROR; + } + mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); -- cgit v1.2.3