dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: add COM_HOME_EN parameter to enable/disable home position

This commit is contained in:
Daniel Agar 2021-12-09 12:14:00 -05:00
parent 28c34a0484
commit 7d7d707db9
3 changed files with 70 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
src/modules/commander/Commander.cpp
View File

@ -944,47 +944,12 @@ Commander::handle_command(const vehicle_command_s &cmd)
break;
case vehicle_command_s::VEHICLE_CMD_DO_SET_HOME: {
bool use_current = cmd.param1 > 0.5f;
if (use_current) {
/* use current position */
if (set_home_position()) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
float yaw = matrix::wrap_2pi(math::radians(cmd.param4));
yaw = PX4_ISFINITE(yaw) ? yaw : (float)NAN;
const double lat = cmd.param5;
const double lon = cmd.param6;
const float alt = cmd.param7;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
const vehicle_local_position_s &local_pos = _local_position_sub.get();
if (local_pos.xy_global && local_pos.z_global) {
/* use specified position */
home_position_s home{};
home.timestamp = hrt_absolute_time();
fillGlobalHomePos(home, lat, lon, alt);
home.manual_home = true;
// update local projection reference including altitude
MapProjection ref_pos{local_pos.ref_lat, local_pos.ref_lon};
float home_x;
float home_y;
ref_pos.project(lat, lon, home_x, home_y);
const float home_z = -(alt - local_pos.ref_alt);
fillLocalHomePos(home, home_x, home_y, home_z, yaw);
/* mark home position as set */
_status_flags.condition_home_position_valid = _home_pub.update(home);
if (_param_com_home_en.get()) {
bool use_current = cmd.param1 > 0.5f;
if (use_current) {
/* use current position */
if (set_home_position()) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
@ -992,8 +957,49 @@ Commander::handle_command(const vehicle_command_s &cmd)
}
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
float yaw = matrix::wrap_2pi(math::radians(cmd.param4));
yaw = PX4_ISFINITE(yaw) ? yaw : (float)NAN;
const double lat = cmd.param5;
const double lon = cmd.param6;
const float alt = cmd.param7;
if (PX4_ISFINITE(lat) && PX4_ISFINITE(lon) && PX4_ISFINITE(alt)) {
const vehicle_local_position_s &local_pos = _local_position_sub.get();
if (local_pos.xy_global && local_pos.z_global) {
/* use specified position */
home_position_s home{};
home.timestamp = hrt_absolute_time();
fillGlobalHomePos(home, lat, lon, alt);
home.manual_home = true;
// update local projection reference including altitude
MapProjection ref_pos{local_pos.ref_lat, local_pos.ref_lon};
float home_x;
float home_y;
ref_pos.project(lat, lon, home_x, home_y);
const float home_z = -(alt - local_pos.ref_alt);
fillLocalHomePos(home, home_x, home_y, home_z, yaw);
/* mark home position as set */
_status_flags.condition_home_position_valid = _home_pub.update(home);
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
} else {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
}
}
} else {
// COM_HOME_EN disabled
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
}
}
break;
@ -1677,7 +1683,8 @@ Commander::set_home_position()
bool
Commander::set_in_air_home_position()
{
if (_status_flags.condition_local_position_valid
if (_param_com_home_en.get()
&& _status_flags.condition_local_position_valid
&& _status_flags.condition_global_position_valid) {
const vehicle_global_position_s &gpos = _global_position_sub.get();
@ -1768,7 +1775,7 @@ Commander::set_home_position_alt_only()
{
const vehicle_local_position_s &lpos = _local_position_sub.get();
if (!_home_pub.get().valid_alt && lpos.z_global) {
if (_param_com_home_en.get() && !_home_pub.get().valid_alt && lpos.z_global) {
// handle special case where we are setting only altitude using local position reference
home_position_s home{};
home.alt = lpos.ref_alt;
@ -1785,12 +1792,14 @@ Commander::set_home_position_alt_only()
void
Commander::updateHomePositionYaw(float yaw)
{
home_position_s home = _home_pub.get();
if (_param_com_home_en.get()) {
home_position_s home = _home_pub.get();
home.yaw = yaw;
home.timestamp = hrt_absolute_time();
home.yaw = yaw;
home.timestamp = hrt_absolute_time();
_home_pub.update(home);
_home_pub.update(home);
}
}
void
@ -2028,7 +2037,7 @@ Commander::run()
}
// automatically set or update home position
if (!_home_pub.get().manual_home) {
if (_param_com_home_en.get() && !_home_pub.get().manual_home) {
// set the home position when taking off, but only if we were previously disarmed
// and at least 500 ms from commander start spent to avoid setting home on in-air restart
if (_should_set_home_on_takeoff && !_land_detector.landed &&
@ -2670,7 +2679,7 @@ Commander::run()
const hrt_abstime now = hrt_absolute_time();
// automatically set or update home position
if (!_home_pub.get().manual_home) {
if (_param_com_home_en.get() && !_home_pub.get().manual_home) {
const vehicle_local_position_s &local_position = _local_position_sub.get();
if (!_armed.armed) {

1
src/modules/commander/Commander.hpp
View File

@ -196,6 +196,7 @@ private:
(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en,
(ParamFloat<px4::params::COM_HOME_H_T>) _param_com_home_h_t,
(ParamFloat<px4::params::COM_HOME_V_T>) _param_com_home_v_t,
(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,

11
src/modules/commander/commander_params.c
View File

@ -202,6 +202,17 @@ PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5f);
*/
PARAM_DEFINE_FLOAT(COM_RCL_ACT_T, 15.0f);
/**
* Home position enabled
*
* Set home position automatically if possible.
*
* @group Commander
* @reboot_required true
* @boolean
*/
PARAM_DEFINE_INT32(COM_HOME_EN, 1);
/**
* Home set horizontal threshold
*