Merge pull request #208 from tritonuas/competition/2024

Competition/2024

.devcontainer/devcontainer.json

@@ -11,6 +11,7 @@
  // "appPort": [ "45906:45906/udp", "45907:45907/udp" ], // port forward airdrop ports for local testing
  "runArgs": [
  "--network=host"
+ // "--device=/dev/ttyACM0"
  ],
 
  "extensions.verifySignature": false,

configs/dev-config.json → configs/dev.json

@@ -4,7 +4,9 @@
  },
  "network": {
  "mavlink": {
- "connect": "tcp://127.0.0.1:14552"
+ "connect": "tcp://localhost:14552",
+ "log_params": false,
+ "telem_poll_rate": 2.0
  },
  "gcs": {
  "port": 5010
@@ -87,7 +89,7 @@
  "gain_auto_lower_limit": 1
  }
  },
- "mavlink_parameters": {
- "Q_GUIDED_MODE": 1 // hover over loiter waypoints
+ "mavlink": {
+ "log_params": false
  }
-}
+}

configs/config.json → configs/jetson.json

@@ -1,17 +1,19 @@
 {
  "logging": {
- "dir": "/obcpp/logs"
+ "dir": "/workspaces/obcpp/logs"
  },
  "network": {
  "mavlink": {
- "connect": "serial:///dev/ttyACM0"
+ "connect": "serial:///dev/ttyACM0",
+ "log_params": false,
+ "telem_poll_rate": 1.0
  },
  "gcs": {
  "port": 5010
  }
  },
  "takeoff": {
- "altitude_m": 30.0
+ "altitude_m": 60.0
  },
  "pathing": {
  "dubins": {
@@ -49,7 +51,7 @@
  },
  "cv": {
  "matching_model_dir": "/obcpp/models/target_siamese_1.pt",
- "segmentation_model_dir": "/obcpp/models/fcn.pth",
+ "segmentation_model_dir": "/obcpp/models/fcn-model_20-epochs_06-01-2023T21-16-02.pth",
  "saliency_model_dir": "/obcpp/models/torchscript_19.pth",
  "not_stolen_addr": "localhost",
  "not_stolen_port": 5069
@@ -87,7 +89,7 @@
  "gain_auto_lower_limit": 1
  }
  },
- "mavlink_parameters": {
- "Q_GUIDED_MODE": 1 // hover over loiter waypoints
+ "mavlink": {
+ "log_params": false
  }
-}
+}

configs/params/stickbug/common.json

@@ -0,0 +1,36 @@
+{ 
+ // Tell the Quadplane to transition to VTOL mode to fulfill loiter waypoint commands,
+ // such as MAV_CMD_NAV_LOITER_TIME.
+ "Q_GUIDED_MODE": 1,
+
+ // Allow a waypoint to be considered "Hit" if the plane travels within 7m of it.
+ "WP_RADIUS": 7,
+
+ /*
+ Enables AFS (Advanced Failsafe Setup)
+
+ Essentially, this tells the plane that after losing RC communications for
+ 3 minutes, it should attempt to terminate flight by crashing.
+
+ However, whether or not the plane will actually execute the terminate command
+ depends on the value of AFS_TERM_ACTION. If AFS_TERM_ACTION is 42 or 43 then
+ the plane will actually terminate flight by initating a crash. If it is any
+ other number, then it won't actually terminate flight.
+
+ We set AFS_TERM_ACTION accordingly in the other, more specific, configs.
+ */
+ "AFS_ENABLE": 1,
+ "AFS_GEOFENCE": 0, // dont terminate flight if outside the geofence
+ "AFS_RC_MAN_ONLY": 0, // do afs RC failsafe in any flight mode, not just manual
+ "AFS_RC_FAIL_TIME": 180.0, // seconds before terminating flight because of RC loss
+
+ /*
+ In contrast to the AFS settings set above, these are the standard failsafe
+ parameters we set.
+ */
+ "THR_FAILSAFE": 1, // enable normal failsafe parameters
+ "FS_SHORT_TIMEOUT": 1.0, // 1 second of comms loss leads to short action, described below
+ "FS_SHORT_ACTN": 0, // continue auto mission, or go to RTL if in manual
+ "FS_LONG_TIMEOUT": 30.0, // 30 seconds of comms loss leads to long action, described below
+ "FS_LONG_ACTN": 1 // RTL
+}

configs/params/stickbug/competition.json

@@ -0,0 +1,4 @@
+{
+ // Magic number to enable flight termination via AFS
+ "AFS_TERM_ACTION": 42
+}

configs/params/stickbug/sitl.json

@@ -0,0 +1,4 @@
+{
+ // Disable flight termination via AFS
+ "AFS_TERM_ACTION": 0
+}

configs/params/stickbug/test-flight.json

@@ -0,0 +1,4 @@
+{
+ // Disable flight termination via AFS
+ "AFS_TERM_ACTION": 0
+}

docker/Makefile

@@ -42,4 +42,5 @@ run-jetson-cuda-check:
 # Note that to re-run cmake inside the container you'll need the really long CMake command
 # in the Dockerfile.jetson.
 jetson-develop:
- cd .. && docker run -it --net=host --runtime=nvidia --platform=linux/arm64 --volume=./:/obcpp --device=/dev/ttyACM0 tritonuas/obcpp:jetson /bin/bash
+ cd .. && docker run -it --net=host --runtime=nvidia --platform=linux/arm64 --volume=./:/obcpp --device=/dev/ttyACM0 ghcr.io/tritonuas/obcpp:jetson /bin/bash
+ # cd .. && docker run -it --net=host --runtime=nvidia --platform=linux/arm64 --volume=./:/obcpp ghcr.io/tritonuas/obcpp:jetson /bin/bash

include/core/mission_state.hpp

@@ -108,6 +108,8 @@ class MissionState {
 
  OBCConfig config;
 
+ std::optional<bottle_t> next_bottle_to_drop;
+
  private:
  std::mutex converter_mut;
  std::optional<CartesianConverter<GPSProtoVec>> converter;
@@ -136,6 +138,7 @@ class MissionState {
  // with the detected_target specified by the index
  std::array<size_t, NUM_AIRDROP_BOTTLES> cv_matches;
 
+
  void _setTick(Tick* newTick); // does not acquire the tick_mut
 };
 

include/network/gcs_routes.hpp

@@ -177,4 +177,16 @@ DEF_GCS_HANDLE(Get, targets, matched);
  */
 DEF_GCS_HANDLE(Post, targets, matched);
 
+/**
+ * POST /kill/kill/kill
+ * 
+ * TELLS THE PLANE TO CRASH ITSELF
+ * 
+ * ONLY CALL THIS IN EMERGENCY SITUATIONS
+ */
+DEF_GCS_HANDLE(Post, kill, kill, kill);
+
+
+DEF_GCS_HANDLE(Get, oh, shit);
+
 #endif // INCLUDE_NETWORK_GCS_ROUTES_HPP_

include/network/mavlink.hpp

@@ -77,10 +77,13 @@ class MavlinkClient {
  mavsdk::Telemetry::FlightMode flight_mode();
  int32_t curr_waypoint() const;
  bool isMissionFinished();
+ bool isAtFinalWaypoint();
  mavsdk::Telemetry::RcStatus get_conn_status();
  bool armAndHover(std::shared_ptr<MissionState> state);
  bool startMission();
 
+ void KILL_THE_PLANE_DO_NOT_CALL_THIS_ACCIDENTALLY();
+
  private:
  mavsdk::Mavsdk mavsdk;
  std::shared_ptr<mavsdk::System> system;

include/pathing/mission_path.hpp

@@ -25,7 +25,7 @@
 class MissionPath {
  public:
  enum class Type {
- FORWARD, HOVER
+ FORWARD, HOVER, HOVER_AT_FINAL
  };
 
  MissionPath(Type type, std::vector<GPSCoord> path, int hover_wait_s = 5);
@@ -42,6 +42,7 @@ class MissionPath {
 
  void generateHoverCommands();
  void generateForwardCommands();
+ void generateHoverAtFinalCommands();
 };
 
 #endif // INCLUDE_PATHING_MISSION_PATH_HPP_

include/ticks/ids.hpp

@@ -34,6 +34,7 @@ constexpr const char* TICK_ID_TO_STR(TickID id) {
  _SET_TICK_ID_MAPPING(FlyWaypoints);
  _SET_TICK_ID_MAPPING(FlySearch);
  _SET_TICK_ID_MAPPING(CVLoiter);
+ _SET_TICK_ID_MAPPING(AirdropPrep);
  _SET_TICK_ID_MAPPING(AirdropApproach);
  _SET_TICK_ID_MAPPING(ManualLanding);
  _SET_TICK_ID_MAPPING(AutoLanding);

include/utilities/constants.hpp

@@ -7,8 +7,13 @@
 #include <string>
 #include <unordered_map>
 
-// Max number of CV pipelines that can be running at the same time
-const size_t MAX_CV_PIPELINES = 5;
+// Max number of CV pipelines that can be running at the same time.
+// NOTE: This number has a large impact on how much memory the OBCpp
+// program uses. Each time a new pipeline is loaded into memory,
+// all the models and other pipeline state must be allocated. This
+// can be on the order of hundreds of megabytes. So, be sure to test
+// how much is reasonable to avoid running out of memory on the Jetson.
+const size_t MAX_CV_PIPELINES = 2;
 
 // common ratios of pi
 const double TWO_PI = 2 * M_PI;

include/utilities/obc_config.hpp

@@ -24,6 +24,8 @@ struct NetworkConfig {
  } gcs;
  struct {
  std::string connect;
+ bool log_params;
+ float telem_poll_rate;
  } mavlink;
 };
 

src/camera/lucid.cpp

@@ -24,6 +24,8 @@ using json = nlohmann::json;
 
 LucidCamera::LucidCamera(CameraConfig config) :
  CameraInterface(config) {
+ this->connect();
+ this->startStreaming();
 }
 
 void LucidCamera::connect() {
@@ -52,7 +54,7 @@ void LucidCamera::connect() {
  break;
  });
 
- LOG_F(ERROR, "Lucid camera connection failed! Retrying in %ld ms",
+ LOG_F(INFO, "Lucid camera connection failed! Retrying in %ld ms",
  this->connectionRetry.count());
  std::this_thread::sleep_for(this->connectionRetry);
  }

src/core/obc.cpp

@@ -39,13 +39,15 @@ OBC::OBC(OBCConfig config) {
  this->state->setCamera(std::make_shared<MockCamera>(this->state->config.camera));
  } else if (this->state->config.camera.type == "lucid") {
  #ifdef ARENA_SDK_INSTALLED
- this->state->setCamera(std::make_shared<LucidCamera>(this->state->config.camera)); //NOLINT
+ this->state->setCamera(std::make_shared<LucidCamera>(this->state->config.camera)); // NOLINT
+ LOG_F(INFO, "Connected to lucid");
  #else
  LOG_F(FATAL, "Attempting to create Lucid Camera without having ArenaSDK installed.");
  #endif
  } else {
  // default to mock if it's neither "mock" or "lucid"
  this->state->setCamera(std::make_shared<MockCamera>(this->state->config.camera));
+ LOG_F(WARNING, "deafault camera config");
  }
 }
 

src/network/airdrop_client.cpp

@@ -82,8 +82,6 @@ void AirdropClient::_establishConnection() {
 }
 
 bool AirdropClient::send(packet_t packet) {
- return true;
-
  set_send_thread();
 
  for (int i = 0; i < 8; i++) {
@@ -197,7 +195,7 @@ void AirdropClient::_receiveWorker() {
  continue;
  }
 
- VLOG_F(TRACE, "RECEIVED AIRDROP PACKET %d %d", (int)packet.header, (int)packet.id);
+ LOG_F(INFO, "RECEIVED AIRDROP PACKET %d %d", (int)packet.header, (int)packet.id);
 
  Lock lock(this->recv_mut);
  this->recv_queue.emplace(packet);

src/network/gcs.cpp

@@ -71,4 +71,8 @@ void GCSServer::_bindHandlers() {
  BIND_HANDLER(Get, targets, all);
  BIND_HANDLER(Get, targets, matched);
  BIND_HANDLER(Post, targets, matched);
+
+ BIND_HANDLER(Post, kill, kill, kill);
+
+ BIND_HANDLER(Get, oh, shit);
 }