aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/px4flow/px4flow.cpp14
-rw-r--r--src/lib/external_lgpl/tecs/tecs.cpp14
-rw-r--r--src/modules/commander/commander.cpp11
-rw-r--r--src/modules/commander/state_machine_helper.cpp35
-rw-r--r--src/modules/navigator/datalinkloss.cpp4
-rw-r--r--src/modules/navigator/mission.cpp28
-rw-r--r--src/modules/navigator/navigator_main.cpp3
-rw-r--r--src/modules/navigator/rcloss.cpp2
-rw-r--r--src/modules/uORB/topics/manual_control_setpoint.h16
-rw-r--r--src/modules/uORB/topics/vehicle_status.h2
10 files changed, 71 insertions, 58 deletions
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 6c68636c6..804027b05 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -76,7 +76,7 @@
#define PX4FLOW_BUS PX4_I2C_BUS_EXPANSION
#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84
//range 0x42 - 0x49
-
+
/* PX4FLOW Registers addresses */
#define PX4FLOW_REG 0x00 /* Measure Register */
@@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
{
// enable debug() calls
- _debug_enabled = true;
+ _debug_enabled = false;
// work_cancel in the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
@@ -240,7 +240,7 @@ PX4FLOW::init()
_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
if (_px4flow_topic < 0) {
- debug("failed to create px4flow object. Did you start uOrb?");
+ warnx("failed to create px4flow object. Did you start uOrb?");
}
ret = OK;
@@ -442,8 +442,6 @@ PX4FLOW::measure()
if (OK != ret) {
perf_count(_comms_errors);
- log("i2c::transfer returned %d", ret);
- printf("i2c::transfer flow returned %d");
return ret;
}
@@ -465,7 +463,7 @@ PX4FLOW::collect()
ret = transfer(nullptr, 0, &val[0], 22);
if (ret < 0) {
- log("error reading from sensor: %d", ret);
+ debug("error reading from sensor: %d", ret);
perf_count(_comms_errors);
perf_end(_sample_perf);
return ret;
@@ -565,7 +563,7 @@ PX4FLOW::cycle()
/* perform collection */
if (OK != collect()) {
- log("collection error");
+ debug("collection error");
/* restart the measurement state machine */
start();
return;
@@ -592,7 +590,7 @@ PX4FLOW::cycle()
/* measurement phase */
if (OK != measure()) {
- log("measure error");
+ debug("measure error");
}
/* next phase is collection */
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0ed210cf4..cfcc48b62 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
}
- // Calculate PD + FF throttle
+ // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+ _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
-
_throttle_dem = constrain(_throttle_dem,
- _last_throttle_dem - thrRateIncr,
- _last_throttle_dem + thrRateIncr);
+ _last_throttle_dem - thrRateIncr,
+ _last_throttle_dem + thrRateIncr);
}
// Ensure _last_throttle_dem is always initialized properly
- // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
-
// Calculate integrator state upper and lower limits
- // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+ // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
- // Set integrator to a max throttle value dduring climbout
+ // Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_climbOutDem) {
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 46caddd46..7917bc9f2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -220,8 +220,6 @@ 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 *status);
-
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
void set_control_mode();
@@ -760,7 +758,10 @@ 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_RCRECOVER] = "AUTO_RCRECOVER";
nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS";
+ nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL] = "AUTO_LANDENGFAIL";
+ nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL] = "AUTO_LANDGPSFAIL";
nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO";
nav_states_str[NAVIGATION_STATE_LAND] = "LAND";
nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND";
@@ -1848,7 +1849,11 @@ int commander_thread_main(int argc, char *argv[])
if (status.failsafe != failsafe_old) {
status_changed = true;
- mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+ if (status.failsafe) {
+ mavlink_log_critical(mavlink_fd, "failsafe mode on");
+ } else {
+ mavlink_log_critical(mavlink_fd, "failsafe mode off");
+ }
failsafe_old = status.failsafe;
}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 9568752ae..e37019d02 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -444,7 +444,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, const bool data_link_loss_enabled, const bool mission_finished,
- const bool stay_in_failsafe)
+ const bool stay_in_failsafe)
{
navigation_state_t nav_state_old = status->nav_state;
@@ -497,11 +497,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
break;
case MAIN_STATE_AUTO_MISSION:
+
/* go into failsafe
* - if commanded to do so
* - if we have an engine failure
- * - if either the datalink is enabled and lost as well as RC is lost
- * - if there is no datalink and the mission is finished */
+ * - depending on datalink, RC and if the mission is finished */
+
+ /* first look at the commands */
if (status->engine_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
} else if (status->data_link_lost_cmd) {
@@ -509,14 +511,17 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
} else if (status->gps_failure_cmd) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->rc_signal_lost_cmd) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
- /* Finished handling commands which have priority , now handle failures */
+ status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+
+ /* finished handling commands which have priority, now handle failures */
} else if (status->gps_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
} else if (status->engine_failure) {
status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
- } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
- (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+
+ /* datalink loss enabled:
+ * check for datalink lost: this should always trigger RTGS */
+ } else if (data_link_loss_enabled && status->data_link_lost) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -529,12 +534,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
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) {
+ /* datalink loss disabled:
+ * check if both, RC and datalink are lost during the mission
+ * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+ } else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+ (status->rc_signal_lost && mission_finished))) {
status->failsafe = true;
if (status->condition_global_position_valid && status->condition_home_position_valid) {
- status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
+ status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
} else if (status->condition_local_position_valid) {
status->nav_state = NAVIGATION_STATE_LAND;
} else if (status->condition_local_altitude_valid) {
@@ -543,13 +551,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
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 && !stay_in_failsafe) {
-
- /* this mode is ok, we don't need RC for missions */
- status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
+ /* stay where you are if you should stay in failsafe, otherwise everything is perfect */
} else if (!stay_in_failsafe){
- /* everything is perfect */
status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
}
break;
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 66f1c8c73..e789fd10d 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:
warnx("dll end");
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7fac69a61..0765e9b7c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -371,7 +371,7 @@ Mission::set_mission_items()
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
- mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
- if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
- /* mission item index out of bounds */
- return false;
- }
-
- /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+ /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+ * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
for (int i = 0; i < 10; i++) {
+
+ if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+ /* mission item index out of bounds */
+ return false;
+ }
+
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (is_current) {
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
- if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+ &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
- "ERROR DO JUMP waypoint could not be written");
+ "ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
- mavlink_log_critical(_navigator->get_mavlink_fd(),
- "DO JUMP repetitions completed");
+ if (is_current) {
+ mavlink_log_critical(_navigator->get_mavlink_fd(),
+ "DO JUMP repetitions completed");
+ }
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@@ -707,6 +712,7 @@ Mission::set_mission_item_reached()
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
}
void
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a867dd0da..e933919fb 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -630,9 +630,6 @@ Navigator::publish_mission_result()
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
- /* reset reached bool */
- _mission_result.reached = false;
- _mission_result.finished = false;
}
void
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 5564a1c42..42392e739 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -163,6 +163,7 @@ RCLoss::advance_rcl()
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
}
break;
case RCL_STATE_LOITER:
@@ -171,6 +172,7 @@ RCLoss::advance_rcl()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE:
warnx("rcl end");
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index dde237adc..50b7bd9e5 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -47,7 +47,7 @@
* Switch position
*/
typedef enum {
- SWITCH_POS_NONE = 0, /**< switch is not mapped */
+ SWITCH_POS_NONE = 0, /**< switch is not mapped */
SWITCH_POS_ON, /**< switch activated (value = 1) */
SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
SWITCH_POS_OFF /**< switch not activated (value = -1) */
@@ -93,13 +93,13 @@ struct manual_control_setpoint_s {
float aux4; /**< default function: camera roll */
float aux5; /**< default function: payload drop */
- switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
- switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
- switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
- switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
- switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
- switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-}; /**< manual control inputs */
+ switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+ switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+ switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
+ switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
+ switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
+ switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
+};
/**
* @}
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index a1b2667e3..91491c148 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -108,7 +108,7 @@ typedef enum {
NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */
NAVIGATION_STATE_ACRO, /**< Acro mode */
NAVIGATION_STATE_LAND, /**< Land mode */
- NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
+ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */
NAVIGATION_STATE_TERMINATION, /**< Termination mode */
NAVIGATION_STATE_OFFBOARD,
NAVIGATION_STATE_MAX,
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-08 18:18:45 +0000