diff --git a/main/messages.proto b/main/messages.proto
index 8d2a41b..f55210c 100644
--- a/main/messages.proto
+++ b/main/messages.proto
@@ -51,6 +51,8 @@ enum GroundCommandType {
   CMD_DRIVE_HEADING = 3;
   CMD_SET_TARGET = 4;
   CMD_SET_CONFIG = 5;
+  CMD_GET_STATUS = 6;
+  CMD_PING = 7;
 }
 
 message DriveHeadingData {
diff --git a/main/ugv.cc b/main/ugv.cc
index 887b885..4d513bd 100644
--- a/main/ugv.cc
+++ b/main/ugv.cc
@@ -10,6 +10,7 @@ static const char *TAG = "ugv_main";
 
 constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
 constexpr float LOOP_PERIOD_S = 1000000.f / static_cast<float>(LOOP_PERIOD_US);
+constexpr TickType_t WATCHDOG_TICKS = pdMS_TO_TICKS(20 * 1000);
 
 void UpdateLocationFromGPS(comms::messages::Location &location,
                            const io::GpsData &        gps_data) {
@@ -92,7 +93,7 @@ void UGV::UpdateAHRS() {
     yaw_ += 360.;
   }
   pitch_ = ahrs_.getPitch();
-  roll_ = ahrs_.getRoll();
+  roll_  = ahrs_.getRoll();
 }
 
 void UGV::DoDebugPrint() {
@@ -110,7 +111,16 @@ void UGV::ReadComms() {
   comms->status.set_yaw_angle(yaw_);
   comms->status.set_pitch_angle(pitch_);
   comms->status.set_roll_angle(roll_);
-  UGV_State comms_ugv_state = comms->status.state();
+  UGV_State  comms_ugv_state = comms->status.state();
+  TickType_t ticks_since_last_packet =
+      (xTaskGetTickCount() - comms->last_packet_tick);
+  if (ticks_since_last_packet >= WATCHDOG_TICKS &&
+      (comms_ugv_state != UGV_State::STATE_IDLE ||
+       current_state_ != UGV_State::STATE_IDLE)) {
+    ESP_LOGW(TAG, "No comms for %f ticks, disabling",
+             ticks_since_last_packet * (1000.f / pdMS_TO_TICKS(1000)));
+    comms_ugv_state = UGV_State::STATE_IDLE;
+  }
   if (comms_ugv_state != current_state_) {
     ESP_LOGI(TAG, "Comms updated UGV state to %d", comms_ugv_state);
     current_state_ = comms_ugv_state;
@@ -138,7 +148,7 @@ void UGV::OnTick() {
   io->ReadInputs(inputs_);
   UpdateAHRS();
 
-  if (time_us >= last_print_ + 200 * 1000) {
+  if (time_us >= last_print_ + 500 * 1000) {
     DoDebugPrint();
     last_print_ = time_us;
   }
@@ -151,7 +161,7 @@ void UGV::OnTick() {
   outputs_.left_motor  = 0.0;
   outputs_.right_motor = 0.0;
 
-  float pitch          = roll_; // TODO: fix quaternion -> YPR
+  float pitch          = roll_;  // TODO: fix quaternion -> YPR
   auto  min_flip_pitch = conf_.min_flip_pitch();
   bool  is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);
 
diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc
index b333cb1..b5c591d 100644
--- a/main/ugv_comms.cc
+++ b/main/ugv_comms.cc
@@ -14,7 +14,7 @@ namespace ugv {
 namespace comms {
 
 static const char *     TAG         = "ugv_comms";
-static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(5000);
+static const TickType_t SEND_PERIOD = pdMS_TO_TICKS(15 * 1000);
 
 CommsClass::CommsClass()
     : last_packet_tick(0),
@@ -121,9 +121,7 @@ void CommsClass::RunTask() {
   using messages::UGV_Status;
 
   TickType_t current_tick = xTaskGetTickCount();
-  next_send               = current_tick + SEND_PERIOD;
-
-  esp_err_t ret;
+  next_status_send        = current_tick;
 
 #ifdef COMMS_SX127X
   sx127x_rx_packet_t rx_packet;
@@ -132,11 +130,8 @@ void CommsClass::RunTask() {
   int         rx_len;
 #endif
 
-  UGV_Message ugv_message;
-  std::string ugv_message_data;
-
   while (true) {
-    TickType_t delay_ticks = next_send - current_tick;
+    TickType_t delay_ticks = next_status_send - current_tick;
 
 #ifdef COMMS_SX127X
     ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
@@ -160,26 +155,31 @@ void CommsClass::RunTask() {
 
     current_tick = xTaskGetTickCount();
 
-    if (current_tick < next_send) {
+    if (current_tick < next_status_send) {
       continue;
     }
 
-    Lock();
-    ugv_message.mutable_status()->CopyFrom(this->status);
-    Unlock();
-    ugv_message.SerializeToString(&ugv_message_data);
-    ret = SendPacket(ugv_message_data);
-    if (ret != ESP_OK) {
-      ESP_LOGE(TAG, "error sending packet: %d", ret);
-    } else {
-      ESP_LOGV(TAG, "lora wrote UGV_Message packet");
-    }
+    SendStatus();
 
     current_tick = xTaskGetTickCount();
-    next_send    = current_tick + SEND_PERIOD;
   }
 }
 
+void CommsClass::SendStatus() {
+  Lock();
+  status_message.mutable_status()->CopyFrom(this->status);
+  Unlock();
+  status_message.SerializeToString(&status_message_data);
+  esp_err_t ret = SendPacket(status_message_data);
+  if (ret != ESP_OK) {
+    ESP_LOGE(TAG, "error sending packet: %d", ret);
+  } else {
+    ESP_LOGV(TAG, "lora wrote UGV_Message packet");
+  }
+
+  next_status_send = xTaskGetTickCount() + SEND_PERIOD;
+}
+
 #ifdef COMMS_SX127X
 void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
   ESP_LOGI(TAG, "lora received packet (len %d, rssi: %d, snr: %f): %.*s\n",
@@ -240,17 +240,28 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
   ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
   using messages::UGV_State;
 
-  xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
   switch (command.type()) {
-    case messages::CMD_DISABLE: status.set_state(UGV_State::STATE_IDLE); break;
+    case messages::CMD_DISABLE:
+      xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+      status.set_state(UGV_State::STATE_IDLE);
+      xSemaphoreGive(mutex);
+      break;
     case messages::CMD_DRIVE_TO_TARGET:
+      xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
       status.set_state(UGV_State::STATE_AQUIRING);
+      xSemaphoreGive(mutex);
+      break;
+    case messages::CMD_TEST:
+      xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
+      status.set_state(UGV_State::STATE_TEST);
+      xSemaphoreGive(mutex);
       break;
-    case messages::CMD_TEST: status.set_state(UGV_State::STATE_TEST); break;
     case messages::CMD_DRIVE_HEADING: {
       if (command.has_drive_heading()) {
+        xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
         status.set_state(UGV_State::STATE_DRIVE_HEADING);
         this->drive_heading = command.drive_heading();
+        xSemaphoreGive(mutex);
       } else {
         status.set_state(UGV_State::STATE_IDLE);
         ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING");
@@ -259,35 +270,42 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
     }
     case messages::CMD_SET_TARGET: {
       if (command.has_target_location()) {
+        xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
         if (!new_target) {
           new_target = new messages::TargetLocation();
         }
         new_target->CopyFrom(command.target_location());
+        xSemaphoreGive(mutex);
       }
       break;
     }
     case messages::CMD_SET_CONFIG: {
       if (command.has_config()) {
+        xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
         if (!new_config) {
           new_config = new config::Config();
         }
         new_config->CopyFrom(command.config());
+        xSemaphoreGive(mutex);
       }
       break;
     }
+    case messages::CMD_GET_STATUS: {
+      SendStatus();
+      break;
+    }
+    case messages::CMD_PING: {
+      break;
+    }
     default:
       ESP_LOGW(TAG, "unhandled command type: %d", command.type());
-      xSemaphoreGive(mutex);
       return;  // early return, no ack
   }
-  xSemaphoreGive(mutex);
 
   messages::UGV_Message ugv_message;
   ugv_message.set_command_ack(command.id());
 
   SendPacket(ugv_message);
-
-  next_send = xTaskGetTickCount() + pdMS_TO_TICKS(200);
 }
 
 esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) {
diff --git a/main/ugv_comms.hh b/main/ugv_comms.hh
index 5b248c6..bf142b5 100644
--- a/main/ugv_comms.hh
+++ b/main/ugv_comms.hh
@@ -52,7 +52,9 @@ class CommsClass {
 #endif
   TaskHandle_t task_handle;
 
-  TickType_t next_send;
+  messages::UGV_Message status_message;
+  std::string status_message_data;
+  TickType_t next_status_send;
 
   void RunTask();
 
@@ -67,6 +69,8 @@ class CommsClass {
   esp_err_t SendPacket(const std::string& str);
   esp_err_t SendPacket(const google::protobuf::MessageLite& message);
 
+  void SendStatus();
+
   static void CommsTask(void* arg);
 };
 
diff --git a/tools/messages_pb2.py b/tools/messages_pb2.py
index 0ed1544..2468ab0 100644
--- a/tools/messages_pb2.py
+++ b/tools/messages_pb2.py
@@ -23,7 +23,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
   package='ugv.messages',
   syntax='proto3',
   serialized_options=_b('H\003'),
-  serialized_pb=_b('\n\x0emessages.proto\x12\x0cugv.messages\x1a\x0c\x63onfig.proto\"5\n\x0eTargetLocation\x12\x10\n\x08latitude\x18\x01 \x01(\x02\x12\x11\n\tlongitude\x18\x02 \x01(\x02\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"\x9a\x01\n\nUGV_Status\x12&\n\x05state\x18\x01 \x01(\x0e\x32\x17.ugv.messages.UGV_State\x12(\n\x08location\x18\x02 \x01(\x0b\x32\x16.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\x12\x13\n\x0bpitch_angle\x18\x04 \x01(\x02\x12\x12\n\nroll_angle\x18\x05 \x01(\x02\"_\n\x0bUGV_Message\x12*\n\x06status\x18\x01 \x01(\x0b\x32\x18.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"2\n\x10\x44riveHeadingData\x12\x0f\n\x07heading\x18\x01 \x01(\x02\x12\r\n\x05power\x18\x02 \x01(\x02\"\xea\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12-\n\x04type\x18\x02 \x01(\x0e\x32\x1f.ugv.messages.GroundCommandType\x12\x37\n\rdrive_heading\x18\x03 \x01(\x0b\x32\x1e.ugv.messages.DriveHeadingDataH\x00\x12\x37\n\x0ftarget_location\x18\x04 \x01(\x0b\x32\x1c.ugv.messages.TargetLocationH\x00\x12$\n\x06\x63onfig\x18\x05 \x01(\x0b\x32\x12.ugv.config.ConfigH\x00\x42\x06\n\x04\x64\x61ta\"Q\n\rGroundMessage\x12.\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1b.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*\xa6\x01\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04\x12\x12\n\x0eSTATE_FLIPPING\x10\x05\x12\x11\n\rSTATE_TURNING\x10\x06\x12\x17\n\x13STATE_DRIVE_HEADING\x10\x07*\x8a\x01\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x12\x15\n\x11\x43MD_DRIVE_HEADING\x10\x03\x12\x12\n\x0e\x43MD_SET_TARGET\x10\x04\x12\x12\n\x0e\x43MD_SET_CONFIG\x10\x05\x42\x02H\x03P\x00\x62\x06proto3')
+  serialized_pb=_b('\n\x0emessages.proto\x12\x0cugv.messages\x1a\x0c\x63onfig.proto\"5\n\x0eTargetLocation\x12\x10\n\x08latitude\x18\x01 \x01(\x02\x12\x11\n\tlongitude\x18\x02 \x01(\x02\"V\n\x08Location\x12\x13\n\x0b\x66ix_quality\x18\x01 \x01(\r\x12\x10\n\x08latitude\x18\x02 \x01(\x02\x12\x11\n\tlongitude\x18\x03 \x01(\x02\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x02\"\x9a\x01\n\nUGV_Status\x12&\n\x05state\x18\x01 \x01(\x0e\x32\x17.ugv.messages.UGV_State\x12(\n\x08location\x18\x02 \x01(\x0b\x32\x16.ugv.messages.Location\x12\x11\n\tyaw_angle\x18\x03 \x01(\x02\x12\x13\n\x0bpitch_angle\x18\x04 \x01(\x02\x12\x12\n\nroll_angle\x18\x05 \x01(\x02\"_\n\x0bUGV_Message\x12*\n\x06status\x18\x01 \x01(\x0b\x32\x18.ugv.messages.UGV_StatusH\x00\x12\x15\n\x0b\x63ommand_ack\x18\x02 \x01(\rH\x00\x42\r\n\x0bugv_message\"2\n\x10\x44riveHeadingData\x12\x0f\n\x07heading\x18\x01 \x01(\x02\x12\r\n\x05power\x18\x02 \x01(\x02\"\xea\x01\n\rGroundCommand\x12\n\n\x02id\x18\x01 \x01(\r\x12-\n\x04type\x18\x02 \x01(\x0e\x32\x1f.ugv.messages.GroundCommandType\x12\x37\n\rdrive_heading\x18\x03 \x01(\x0b\x32\x1e.ugv.messages.DriveHeadingDataH\x00\x12\x37\n\x0ftarget_location\x18\x04 \x01(\x0b\x32\x1c.ugv.messages.TargetLocationH\x00\x12$\n\x06\x63onfig\x18\x05 \x01(\x0b\x32\x12.ugv.config.ConfigH\x00\x42\x06\n\x04\x64\x61ta\"Q\n\rGroundMessage\x12.\n\x07\x63ommand\x18\x01 \x01(\x0b\x32\x1b.ugv.messages.GroundCommandH\x00\x42\x10\n\x0eground_message*\xa6\x01\n\tUGV_State\x12\x0e\n\nSTATE_IDLE\x10\x00\x12\x12\n\x0eSTATE_AQUIRING\x10\x01\x12\x11\n\rSTATE_DRIVING\x10\x02\x12\x12\n\x0eSTATE_FINISHED\x10\x03\x12\x0e\n\nSTATE_TEST\x10\x04\x12\x12\n\x0eSTATE_FLIPPING\x10\x05\x12\x11\n\rSTATE_TURNING\x10\x06\x12\x17\n\x13STATE_DRIVE_HEADING\x10\x07*\xac\x01\n\x11GroundCommandType\x12\x0f\n\x0b\x43MD_DISABLE\x10\x00\x12\x17\n\x13\x43MD_DRIVE_TO_TARGET\x10\x01\x12\x0c\n\x08\x43MD_TEST\x10\x02\x12\x15\n\x11\x43MD_DRIVE_HEADING\x10\x03\x12\x12\n\x0e\x43MD_SET_TARGET\x10\x04\x12\x12\n\x0e\x43MD_SET_CONFIG\x10\x05\x12\x12\n\x0e\x43MD_GET_STATUS\x10\x06\x12\x0c\n\x08\x43MD_PING\x10\x07\x42\x02H\x03P\x00\x62\x06proto3')
   ,
   dependencies=[config__pb2.DESCRIPTOR,],
   public_dependencies=[config__pb2.DESCRIPTOR,])
@@ -105,11 +105,19 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
       name='CMD_SET_CONFIG', index=5, number=5,
       serialized_options=None,
       type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_GET_STATUS', index=6, number=6,
+      serialized_options=None,
+      type=None),
+    _descriptor.EnumValueDescriptor(
+      name='CMD_PING', index=7, number=7,
+      serialized_options=None,
+      type=None),
   ],
   containing_type=None,
   serialized_options=None,
   serialized_start=985,
-  serialized_end=1123,
+  serialized_end=1157,
 )
 _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
 
@@ -128,6 +136,8 @@ CMD_TEST = 2
 CMD_DRIVE_HEADING = 3
 CMD_SET_TARGET = 4
 CMD_SET_CONFIG = 5
+CMD_GET_STATUS = 6
+CMD_PING = 7
 
 
 
diff --git a/tools/ugv_cmd.py b/tools/ugv_cmd.py
index dd405a9..ceaa8a3 100755
--- a/tools/ugv_cmd.py
+++ b/tools/ugv_cmd.py
@@ -120,8 +120,8 @@ class UGV_CLI:
         self.ugv.write_command(cmd)
         log.info("driving to target")
 
-    @cli_cmd(names=["get_status", "s"], description="Print the last status of the UGV")
-    def get_status(self):
+    @cli_cmd(names=["last_status", "ls", "s"], description="Print the last status of the UGV")
+    def last_status(self):
         if self.ugv.last_status_time is None:
             log.info("no status received")
         else:
@@ -129,6 +129,20 @@ class UGV_CLI:
             log.info("last status (%.4f seconds ago): %s",
                      last_status_delay, self.ugv.last_status)
 
+    @cli_cmd(names=["get_status", "gs"], description="Get the current status of the UGV")
+    def get_status(self):
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_GET_STATUS
+        self.ugv.write_command(cmd)
+        self.last_status()
+
+    @cli_cmd(names=["ping", "p"], description="Ping the UGV")
+    def get_status(self):
+        cmd = messages.GroundCommand()
+        cmd.type = messages.CMD_PING
+        self.ugv.write_command(cmd)
+        print("Received ping response")
+
     @staticmethod
     def find_command(name):
         for cmd in cli_commands:
@@ -167,6 +181,7 @@ class UGV_CLI:
             while self.is_running:
                 line = input("UGV> ")
                 if len(line) is 0 and last_line is not None:
+                    print(last_line)
                     line = last_line
                 line_parts = line.split(' ')
                 if len(line_parts) is 0: