diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a9f6a23513..064e803997 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,13 +1398,13 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta /* land switch */ if (!isfinite(sp_man->return_switch)) { - current_status->return_switch = RETURN_SWITCH_NONE; + current_status->return_switch = SWITCH_OFF; } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { - current_status->return_switch = RETURN_SWITCH_RETURN; + current_status->return_switch = SWITCH_ON; } else { - current_status->return_switch = RETURN_SWITCH_NONE; + current_status->return_switch = SWITCH_OFF; } /* assisted switch */ @@ -1418,7 +1418,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; } - /* mission switch */ + /* mission switch */ if (!isfinite(sp_man->mission_switch)) { current_status->mission_switch = MISSION_SWITCH_MISSION; @@ -1428,6 +1428,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { current_status->mission_switch = MISSION_SWITCH_MISSION; } + + /* distance bottom switch */ + if (!isfinite(sp_man->dist_bottom_switch)) { + current_status->dist_bottom_switch = SWITCH_OFF; + + } else if (sp_man->dist_bottom_switch > STICK_ON_OFF_LIMIT) { + current_status->dist_bottom_switch = SWITCH_ON; + + } else { + current_status->dist_bottom_switch = SWITCH_OFF; + } } transition_result_t @@ -1548,7 +1559,7 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c /* 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) { + if (status->return_switch == SWITCH_ON) { /* RTL */ res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 58f7238ff3..579a0d3d1f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -424,6 +424,16 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t } } + bool use_dist_bottom_prev = control_mode->flag_use_dist_bottom; + control_mode->flag_use_dist_bottom = control_mode->flag_control_manual_enabled && + control_mode->flag_control_altitude_enabled && status->dist_bottom_switch == SWITCH_ON; + + if (ret == TRANSITION_NOT_CHANGED && control_mode->flag_use_dist_bottom != use_dist_bottom_prev) { + // TODO really, navigation state not changed, set this to force publishing control_mode + ret = TRANSITION_CHANGED; + navigation_state_changed = true; + } + return ret; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 7abeade80e..f283a1eb45 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -229,14 +229,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) bool reset_auto_sp_xy = true; bool reset_auto_sp_z = true; bool reset_takeoff_sp = true; + bool reset_z_sp_dist = false; + float surface_offset = 0.0f; // Z of ground level + float z_sp_dist = 0.0f; // distance to ground setpoint (positive) hrt_abstime t_prev = 0; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; - float ref_alt_prev = 0.0f; - hrt_abstime ref_surface_prev_t = 0; uint64_t local_ref_timestamp = 0; + uint64_t surface_bottom_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; @@ -250,7 +252,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); - for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); @@ -345,28 +346,48 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (control_mode.flag_control_manual_enabled) { /* manual control */ /* check for reference point updates and correct setpoint */ - if (local_pos.ref_timestamp != ref_surface_prev_t) { + //if (local_pos.ref_timestamp != ref_prev_t) { /* reference alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; - + //local_pos_sp.z += local_pos.ref_alt - ref_alt_prev; // TODO also correct XY setpoint - } + //} - /* reset setpoints to current position if needed */ if (control_mode.flag_control_altitude_enabled) { + /* reset setpoints to current position if needed */ if (reset_man_sp_z) { reset_man_sp_z = false; local_pos_sp.z = local_pos.z; + reset_z_sp_dist = true; mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z); } + /* if distance to surface is available, use it */ + if (control_mode.flag_use_dist_bottom && local_pos.dist_bottom_valid) { + if (reset_z_sp_dist) { + reset_z_sp_dist = false; + surface_offset = local_pos.z + local_pos.dist_bottom; + z_sp_dist = -local_pos_sp.z + surface_offset; + mavlink_log_info(mavlink_fd, "[mpc] surface offset: %.2f", surface_offset); + } else { + if (local_pos.surface_bottom_timestamp != surface_bottom_timestamp) { + /* new surface level */ + z_sp_dist += local_pos.z + local_pos.dist_bottom - surface_offset; + } + surface_offset = local_pos.z + local_pos.dist_bottom; + } + /* move altitude setpoint according to ground distance */ + local_pos_sp.z = surface_offset - z_sp_dist; + } + /* move altitude setpoint with throttle stick */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; + z_sp_dist -= sp_move_rate[2] * dt; // this will have effect only if distance sensor used + // TODO add z_sp_dist correction here if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; @@ -673,13 +694,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_auto_sp_z = true; } - /* track local position reference even when not controlling position */ - ref_surface_prev_t = local_pos.ref_timestamp; - ref_alt_prev = local_pos.ref_alt; - /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ reset_int_z_manual = control_mode.flag_armed && control_mode.flag_control_manual_enabled && !control_mode.flag_control_climb_rate_enabled; + /* reset distance setpoint if distance is not available */ + if (!local_pos.dist_bottom_valid) { + reset_z_sp_dist = true; + } + + surface_bottom_timestamp = local_pos.surface_bottom_timestamp; + /* run at approximately 100 Hz */ usleep(10000); } 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 6df7c69b2f..8cd161075d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -439,12 +439,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { /* new ground level */ surface_offset -= sonar_corr; - mavlink_log_info(mavlink_fd, "[inav] new surface level: %.3f", surface_offset); - alt_avg -= sonar_corr; // TODO check this sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; 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); } } else { @@ -521,14 +521,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (gps_valid) { if (gps.eph_m > 10.0f) { gps_valid = false; - warnx("GPS signal lost"); mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { if (gps.eph_m < 5.0f) { gps_valid = true; - warnx("GPS signal found"); mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); } } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 4d719e0e15..6f5c20d391 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -188,6 +188,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_DIST_B_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 085da905c6..78b2fa8eee 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -315,6 +315,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; + int rc_map_dist_bottom_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -360,6 +361,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; + param_t rc_map_dist_bottom_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -578,6 +580,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_dist_bottom_sw = param_find("RC_MAP_DIST_B_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -726,6 +729,10 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_dist_bottom_sw, &(_parameters.rc_map_dist_bottom_sw)) != OK) { + warnx("Failed getting distance bottom sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } @@ -754,6 +761,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[DIST_BOTTOM] = _parameters.rc_map_dist_bottom_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1330,6 +1338,8 @@ Sensors::ppm_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; + manual_control.dist_bottom_switch = NAN; + // manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; @@ -1442,6 +1452,9 @@ Sensors::ppm_poll() /* mission switch input */ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); + /* distance bottom switch input */ + manual_control.dist_bottom_switch = limit_minus_one_to_one(_rc.chan[_rc.function[DIST_BOTTOM]].scaled); + /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1460,6 +1473,10 @@ Sensors::ppm_poll() manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + if (_rc.function[DIST_BOTTOM] >= 0) { + manual_control.dist_bottom_switch = limit_minus_one_to_one(_rc.chan[_rc.function[DIST_BOTTOM]].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/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e986..6e14d4beec 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 dist_bottom_switch; /**< distance bottom 2 position switch (optional): off, on */ /** * 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 5a85801439..8b952c28f1 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -63,20 +63,21 @@ enum RC_CHANNELS_FUNCTION { THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, + ROLL, + PITCH, + YAW, + MODE, + RETURN, + ASSISTED, + MISSION, + DIST_BOTTOM, + OFFBOARD_MODE, + FLAPS, + AUX_1, + AUX_2, + AUX_3, + AUX_4, + AUX_5, 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_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d1..cd004844cf 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -79,6 +79,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_use_dist_bottom; /**< true if bottom distance sensor should be used for altitude control */ 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 6bc63cbaea..96ae9cbcec 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -106,16 +106,16 @@ typedef enum { ASSISTED_SWITCH_EASY } assisted_switch_pos_t; -typedef enum { - RETURN_SWITCH_NONE = 0, - RETURN_SWITCH_RETURN -} return_switch_pos_t; - typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + SWITCH_OFF = 0, + SWITCH_ON +} on_off_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -187,9 +187,10 @@ struct vehicle_status_s bool is_rotary_wing; mode_switch_pos_t mode_switch; - return_switch_pos_t return_switch; + on_off_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + on_off_switch_pos_t dist_bottom_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */