fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-29 16:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added "bottom distance" switch, multirotor_pos_control implemented: use bottom distance to surface to control altitde

Browse Source
This commit is contained in:
Anton Babushkin 2013-10-11 11:38:36 +02:00
parent 86c53bee43
commit dc09182b95
10 changed files with 106 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/commander/commander.cpp
View File

@ -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);

10
src/modules/commander/state_machine_helper.cpp
View File

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

48
src/modules/multirotor_pos_control/multirotor_pos_control.c
View File

@ -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(&params_h);
parameters_update(&params_h, &params);
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);
}

6
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -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");
}
}

1
src/modules/sensors/sensor_params.c
View File

@ -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);

17
src/modules/sensors/sensors.cpp
View File

@ -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);
// }

1
src/modules/uORB/topics/manual_control_setpoint.h
View File

@ -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

29
src/modules/uORB/topics/rc_channels.h
View File

@ -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. */
};

1
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -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

13
src/modules/uORB/topics/vehicle_status.h
View File

@ -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 */