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

even more good commands

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-23 18:55:20 -07:00
parent 771e3580c4
commit 068cd2d33b
6 changed files with 97 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/messages.proto
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
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);

76
main/ugv_comms.cc
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:
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_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; 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
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
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
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: