-
Notifications
You must be signed in to change notification settings - Fork 337
Don't send direct mode changes from the planner #220
Conversation
@@ -50,17 +50,9 @@ int main(int argc, char** argv) { | |||
if (since_last_cloud > pointcloud_timeout_land && | |||
since_start > pointcloud_timeout_land) { | |||
if (!landing) { | |||
mavros_msgs::SetMode mode_msg; | |||
mode_msg.request.custom_mode = "AUTO.LOITER"; | |||
landing = true; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Since we are now not handling landing/loiter etc, can we get rid of this boolean altogether?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
hmm I don't see a way. We still need it to decide whether to send out stuff or not. The Firmware will only go to loiter if we stop sending. But the name of this variable is now outdated (for the second time) maybe we should rename the bool to aborting?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
or even more verbose planner_is_healthy
? (Will of course flip the true/false semantic of it). I like booleans to have "is" in their name, making it clear that they are booleans
What is the response time of this relative to the timeout on the firmware? Phrased another way, if a depth camera input is lost, how long would it take the firmware to go to hold before vs. after this PR? |
The Firmware will switch out of the mission mode once the elapsed time since the last received trajectory message is bigger than TRAJECTORY_STREAM_TIMEOUT_US (right now hardcoded to 500_ms) Before this PR, the planner switched to hover after it did not receive input data since pointcloud_timeout_land (parameter, default value 15 seconds). So this was not meant to be a safety catch in the first place. |
f9103d8
to
4bc7aae
Compare
The local planner should not be able to force mode changes. The planner was able to force the drone to hover if it was not getting sensor data. No matter whether the avoidance was even enabled or if the drone was in mission. This produced situations, where the drone constantly switched from position back to hover because to local_planner (running in the background) was struggling.
Sending hover from the planner is not necessary anymore anyway as PR #11187 handles failsafe actions.