amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

even more good commands

master
Alex Mikhalev 6 years ago
parent
771e3580c4
commit
068cd2d33b
6 changed files with 95 additions and 36 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      main/messages.proto
  2. 18
      main/ugv.cc
  3. 72
      main/ugv_comms.cc
  4. 6
      main/ugv_comms.hh
  5. 14
      tools/messages_pb2.py
  6. 19
      tools/ugv_cmd.py

2
main/messages.proto
Unescape View File

@ -51,6 +51,8 @@ enum GroundCommandType {
CMD_DRIVE_HEADING = 3; CMD_DRIVE_HEADING = 3;
CMD_SET_TARGET = 4; CMD_SET_TARGET = 4;
CMD_SET_CONFIG = 5; CMD_SET_CONFIG = 5;
CMD_GET_STATUS = 6;
CMD_PING = 7;
} }
message DriveHeadingData { message DriveHeadingData {

18
main/ugv.cc
Unescape View File

@ -10,6 +10,7 @@ static const char *TAG = "ugv_main";
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
constexpr float LOOP_PERIOD_S = 1000000.f / static_cast<float>(LOOP_PERIOD_US); 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, void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data) { const io::GpsData & gps_data) {
@ -92,7 +93,7 @@ void UGV::UpdateAHRS() {
yaw_ += 360.; yaw_ += 360.;
} }
pitch_ = ahrs_.getPitch(); pitch_ = ahrs_.getPitch();
roll_ = ahrs_.getRoll(); roll_ = ahrs_.getRoll();
} }
void UGV::DoDebugPrint() { void UGV::DoDebugPrint() {
@ -110,7 +111,16 @@ void UGV::ReadComms() {
comms->status.set_yaw_angle(yaw_); comms->status.set_yaw_angle(yaw_);
comms->status.set_pitch_angle(pitch_); comms->status.set_pitch_angle(pitch_);
comms->status.set_roll_angle(roll_); 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_) { if (comms_ugv_state != current_state_) {
ESP_LOGI(TAG, "Comms updated UGV state to %d", comms_ugv_state); ESP_LOGI(TAG, "Comms updated UGV state to %d", comms_ugv_state);
current_state_ = comms_ugv_state; current_state_ = comms_ugv_state;
@ -138,7 +148,7 @@ void UGV::OnTick() {
io->ReadInputs(inputs_); io->ReadInputs(inputs_);
UpdateAHRS(); UpdateAHRS();
if (time_us >= last_print_ + 200 * 1000) { if (time_us >= last_print_ + 500 * 1000) {
DoDebugPrint(); DoDebugPrint();
last_print_ = time_us; last_print_ = time_us;
} }
@ -151,7 +161,7 @@ void UGV::OnTick() {
outputs_.left_motor = 0.0; outputs_.left_motor = 0.0;
outputs_.right_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(); auto min_flip_pitch = conf_.min_flip_pitch();
bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch); bool is_upside_down = (pitch > min_flip_pitch) || (pitch < -min_flip_pitch);

72
main/ugv_comms.cc
Unescape View File

@ -14,7 +14,7 @@ namespace ugv {
namespace comms { namespace comms {
static const char * TAG = "ugv_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() CommsClass::CommsClass()
: last_packet_tick(0), : last_packet_tick(0),
@ -121,9 +121,7 @@ void CommsClass::RunTask() {
using messages::UGV_Status; using messages::UGV_Status;
TickType_t current_tick = xTaskGetTickCount(); TickType_t current_tick = xTaskGetTickCount();
next_send = current_tick + SEND_PERIOD; next_status_send = current_tick;
esp_err_t ret;
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_rx_packet_t rx_packet; sx127x_rx_packet_t rx_packet;
@ -132,11 +130,8 @@ void CommsClass::RunTask() {
int rx_len; int rx_len;
#endif #endif
UGV_Message ugv_message;
std::string ugv_message_data;
while (true) { while (true) {
TickType_t delay_ticks = next_send - current_tick; TickType_t delay_ticks = next_status_send - current_tick;
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks); ret = sx127x_recv_packet(lora, &rx_packet, delay_ticks);
@ -160,26 +155,31 @@ void CommsClass::RunTask() {
current_tick = xTaskGetTickCount(); current_tick = xTaskGetTickCount();
if (current_tick < next_send) { if (current_tick < next_status_send) {
continue; continue;
} }
Lock(); SendStatus();
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");
}
current_tick = xTaskGetTickCount(); 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 #ifdef COMMS_SX127X
void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) { void CommsClass::HandlePacket(sx127x_rx_packet_t *rx_packet) {
ESP_LOGI(TAG, "lora received packet (len %d, rssi: %d, snr: %f): %.*s\n", 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()); ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
using messages::UGV_State; using messages::UGV_State;
xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
switch (command.type()) { 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: case messages::CMD_DRIVE_TO_TARGET:
xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
status.set_state(UGV_State::STATE_AQUIRING); 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; break;
case messages::CMD_TEST: status.set_state(UGV_State::STATE_TEST); break;
case messages::CMD_DRIVE_HEADING: { case messages::CMD_DRIVE_HEADING: {
if (command.has_drive_heading()) { if (command.has_drive_heading()) {
xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
status.set_state(UGV_State::STATE_DRIVE_HEADING); status.set_state(UGV_State::STATE_DRIVE_HEADING);
this->drive_heading = command.drive_heading(); this->drive_heading = command.drive_heading();
xSemaphoreGive(mutex);
} else { } else {
status.set_state(UGV_State::STATE_IDLE); status.set_state(UGV_State::STATE_IDLE);
ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING"); ESP_LOGE(TAG, "invalid CMD_DRIVE_HEADING");
@ -259,35 +270,42 @@ void CommsClass::HandleCommand(const messages::GroundCommand &command) {
} }
case messages::CMD_SET_TARGET: { case messages::CMD_SET_TARGET: {
if (command.has_target_location()) { if (command.has_target_location()) {
xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
if (!new_target) { if (!new_target) {
new_target = new messages::TargetLocation(); new_target = new messages::TargetLocation();
} }
new_target->CopyFrom(command.target_location()); new_target->CopyFrom(command.target_location());
xSemaphoreGive(mutex);
} }
break; break;
} }
case messages::CMD_SET_CONFIG: { case messages::CMD_SET_CONFIG: {
if (command.has_config()) { if (command.has_config()) {
xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
if (!new_config) { if (!new_config) {
new_config = new config::Config(); new_config = new config::Config();
} }
new_config->CopyFrom(command.config()); new_config->CopyFrom(command.config());
xSemaphoreGive(mutex);
} }
break; break;
} }
case messages::CMD_GET_STATUS: {
SendStatus();
break;
}
case messages::CMD_PING: {
break;
}
default: default:
ESP_LOGW(TAG, "unhandled command type: %d", command.type()); ESP_LOGW(TAG, "unhandled command type: %d", command.type());
xSemaphoreGive(mutex);
return; // early return, no ack return; // early return, no ack
} }
xSemaphoreGive(mutex);
messages::UGV_Message ugv_message; messages::UGV_Message ugv_message;
ugv_message.set_command_ack(command.id()); ugv_message.set_command_ack(command.id());
SendPacket(ugv_message); SendPacket(ugv_message);
next_send = xTaskGetTickCount() + pdMS_TO_TICKS(200);
} }
esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) { esp_err_t CommsClass::SendPacket(const uint8_t *data, size_t size) {

6
main/ugv_comms.hh
Unescape View File

@ -52,7 +52,9 @@ class CommsClass {
#endif #endif
TaskHandle_t task_handle; 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(); void RunTask();
@ -67,6 +69,8 @@ class CommsClass {
esp_err_t SendPacket(const std::string& str); esp_err_t SendPacket(const std::string& str);
esp_err_t SendPacket(const google::protobuf::MessageLite& message); esp_err_t SendPacket(const google::protobuf::MessageLite& message);
void SendStatus();
static void CommsTask(void* arg); static void CommsTask(void* arg);
}; };

14
tools/messages_pb2.py
Unescape View File

@ -23,7 +23,7 @@ DESCRIPTOR = _descriptor.FileDescriptor(
package='ugv.messages', package='ugv.messages',
syntax='proto3', syntax='proto3',
serialized_options=_b('H\003'), 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,], dependencies=[config__pb2.DESCRIPTOR,],
public_dependencies=[config__pb2.DESCRIPTOR,]) public_dependencies=[config__pb2.DESCRIPTOR,])
@ -105,11 +105,19 @@ _GROUNDCOMMANDTYPE = _descriptor.EnumDescriptor(
name='CMD_SET_CONFIG', index=5, number=5, name='CMD_SET_CONFIG', index=5, number=5,
serialized_options=None, serialized_options=None,
type=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, containing_type=None,
serialized_options=None, serialized_options=None,
serialized_start=985, serialized_start=985,
serialized_end=1123, serialized_end=1157,
) )
_sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE) _sym_db.RegisterEnumDescriptor(_GROUNDCOMMANDTYPE)
@ -128,6 +136,8 @@ CMD_TEST = 2
CMD_DRIVE_HEADING = 3 CMD_DRIVE_HEADING = 3
CMD_SET_TARGET = 4 CMD_SET_TARGET = 4
CMD_SET_CONFIG = 5 CMD_SET_CONFIG = 5
CMD_GET_STATUS = 6
CMD_PING = 7

19
tools/ugv_cmd.py
Unescape View File

@ -120,8 +120,8 @@ class UGV_CLI:
self.ugv.write_command(cmd) self.ugv.write_command(cmd)
log.info("driving to target") log.info("driving to target")
@cli_cmd(names=["get_status", "s"], description="Print the last status of the UGV") @cli_cmd(names=["last_status", "ls", "s"], description="Print the last status of the UGV")
def get_status(self): def last_status(self):
if self.ugv.last_status_time is None: if self.ugv.last_status_time is None:
log.info("no status received") log.info("no status received")
else: else:
@ -129,6 +129,20 @@ class UGV_CLI:
log.info("last status (%.4f seconds ago): %s", log.info("last status (%.4f seconds ago): %s",
last_status_delay, self.ugv.last_status) 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 @staticmethod
def find_command(name): def find_command(name):
for cmd in cli_commands: for cmd in cli_commands:
@ -167,6 +181,7 @@ class UGV_CLI:
while self.is_running: while self.is_running:
line = input("UGV> ") line = input("UGV> ")
if len(line) is 0 and last_line is not None: if len(line) is 0 and last_line is not None:
print(last_line)
line = last_line line = last_line
line_parts = line.split(' ') line_parts = line.split(' ')
if len(line_parts) is 0: if len(line_parts) is 0:

Write Preview
Loading…
Cancel
Save