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

Implement initial driving to target logic

Browse Source
This commit is contained in:
Alex Mikhalev 2019-03-31 20:05:33 -07:00
parent 2233eeff4e
commit b55eb69eb8
4 changed files with 149 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
e32_client/ugv.py
View File

@ -30,6 +30,7 @@ class UGVComms(E32):
msg_len = int.from_bytes(len_data, byteorder='big')
data = self.ser.read(size=msg_len)
if len(data) != msg_len:
print("read bad data: ", data)
self.ser.flush()
return None
msg = messages.UGV_Message()
@ -49,7 +50,7 @@ def __rx_thread_entry(ugv: UGVComms):
if __name__ == "__main__":
ser = serial.serial_for_url("loop://", baudrate=9600, parity=serial.PARITY_NONE,
ser = serial.serial_for_url("/dev/ttyUSB1", baudrate=9600, parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
timeout=2.0)
ugv = UGVComms(ser)

14
main/messages.proto
View File

@ -5,10 +5,11 @@ package uas.ugv.messages;
option optimize_for = LITE_RUNTIME;
enum UGV_State {
IDLE = 0;
AQUIRING = 1;
DRIVING = 2;
FINISHED = 3;
STATE_IDLE = 0;
STATE_AQUIRING = 1;
STATE_DRIVING = 2;
STATE_FINISHED = 3;
STATE_TEST = 4;
}
message Location {
@ -31,8 +32,9 @@ message UGV_Message {
}
enum GroundCommandType {
DISABLE = 0;
ENABLE = 1;
CMD_DISABLE = 0;
CMD_DRIVE_TO_TARGET = 1;
CMD_TEST = 2;
}
message GroundCommand {

27
main/ugv_comms.cc
View File

@ -16,7 +16,7 @@ static const char *TAG = "ugv_comms";
CommsClass::CommsClass()
: location(),
ugv_state(messages::IDLE),
ugv_state(messages::STATE_IDLE),
last_packet_tick(0),
last_packet_rssi(INT32_MIN),
last_packet_snr(INT8_MIN) {
@ -30,7 +30,7 @@ void CommsClass::Init() {
location.set_latitude(43.65);
location.set_longitude(-116.20);
location.set_altitude(2730);
ugv_state = messages::UGV_State::IDLE;
ugv_state = messages::UGV_State::STATE_IDLE;
#ifdef COMMS_SX127X
sx127x_config_t lora_config = sx127x_config_default();
@ -145,18 +145,18 @@ void CommsClass::RunTask() {
sx127x_packet_rx_free(&rx_packet);
}
#else
if (packet_len <= 0) { // need to receive packet size first
if (packet_len <= 0) { // need to receive packet size first
rx_len = lora.Read(rx_buf, 1, delay_ticks);
if (rx_len > 0) {
packet_len = rx_buf[0];
if (packet_len > MAX_PACKET_LEN) { // should not be possible
if (packet_len > MAX_PACKET_LEN) { // should not be possible
ESP_LOGE(TAG, "invalid packet len received: %d", packet_len);
packet_len = -1;
} else {
ESP_LOGV(TAG, "rx packet len: %d", packet_len);
}
}
} else { // receiving packet data now
} else { // receiving packet data now
rx_len = lora.Read(rx_buf, packet_len, PACKET_RX_TIMEOUT);
if (rx_len < packet_len) {
ESP_LOGE(TAG, "timeout for packet rx");
@ -165,7 +165,7 @@ void CommsClass::RunTask() {
ESP_LOGV(TAG, "rx data: %.*s", packet_len, rx_buf);
HandlePacket(rx_buf, packet_len);
}
packet_len = -1; // wait for new packet len
packet_len = -1; // wait for new packet len
}
// TODO: checksum?
#endif
@ -239,7 +239,20 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
void CommsClass::HandleCommand(const messages::GroundCommand &command) {
ESP_LOGI(TAG, "rx command id %d type %d", command.id(), command.type());
// TODO: handle command
xSemaphoreTake(mutex, pdMS_TO_TICKS(10));
switch (command.type()) {
case messages::CMD_DISABLE: ugv_state = messages::STATE_IDLE; break;
case messages::CMD_DRIVE_TO_TARGET:
ugv_state = messages::STATE_AQUIRING;
break;
case messages::CMD_TEST: ugv_state = messages::STATE_TEST; 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());

124
main/ugv_main.cc
View File

@ -11,6 +11,7 @@
namespace ugv {
using ugv::comms::CommsClass;
using ugv::comms::messages::UGV_State;
using ugv::io::IOClass;
static const char *TAG = "ugv_main";
@ -20,10 +21,71 @@ SemaphoreHandle_t i2c_mutex;
}
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100;
static const float PI = atanf(1.0) * 4.0;
static const float PI =
3.1415926535897932384626433832795028841971693993751058209749445923078164062;
extern "C" void OnTimeout(void *arg);
void UpdateLocationFromGPS(comms::messages::Location &location,
const io::GpsData & gps_data) {
location.set_fix_quality(gps_data.fix_quality);
location.set_latitude(gps_data.latitude);
location.set_longitude(gps_data.longitude);
location.set_altitude(gps_data.altitude);
}
static const float RAD_PER_DEG = PI / 180.f;
// Radius of earth in meters
static const float EARTH_RAD = 6372795.f;
static const float DRIVE_POWER = 0.5;
static const float ANGLE_P = 0.02;
static const float MIN_DIST = 10.0;
struct LatLong {
public:
float latitude;
float longitude;
inline LatLong(double latitude_, double longitude_)
: latitude(latitude_), longitude(longitude_) {}
/**
* Return distance from this LatLong to target, in meters
*/
float distance_to(const LatLong &target) const {
float lat1 = latitude * RAD_PER_DEG;
float lat2 = target.latitude * RAD_PER_DEG;
float long1 = longitude * RAD_PER_DEG;
float long2 = target.longitude * RAD_PER_DEG;
float clat1 = cosf(lat1);
float clat2 = cosf(lat2);
float a = powf(sinf((long2 - long1) / 2.f), 2.f) * clat1 * clat2 +
powf(sinf((lat2 - lat1) / 2.f), 2.f);
float d_over_r = 2 * atan2f(sqrtf(a), sqrtf(1 - a));
return d_over_r * EARTH_RAD;
}
float bearing_toward(const LatLong &target) const {
float dlong = (target.longitude - longitude) * RAD_PER_DEG;
float sdlong = sinf(dlong);
float cdlong = cosf(dlong);
float lat1 = latitude * RAD_PER_DEG;
float lat2 = target.latitude * RAD_PER_DEG;
float slat1 = sinf(lat1);
float clat1 = cosf(lat1);
float slat2 = sinf(lat2);
float clat2 = cosf(lat2);
float num = sdlong * clat2;
float denom = (clat1 * slat2) - (slat1 * clat2 * cdlong);
float course = atan2f(num, denom);
if (course < 0.0) {
course += 2 * PI;
}
return course / RAD_PER_DEG;
}
};
struct State {
public:
CommsClass * comms;
@ -34,8 +96,9 @@ struct State {
io::Outputs outputs;
int64_t last_print;
Madgwick ahrs_;
LatLong target;
State() {
State() : target{34.069022, -118.443067} {
comms = new CommsClass();
io = new IOClass();
display = new DisplayClass(comms);
@ -67,9 +130,6 @@ struct State {
int64_t time_us = esp_timer_get_time();
float time_s = ((float)time_us) / 1e6;
io->ReadInputs(inputs);
outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI);
io->WriteOutputs(outputs);
{
io::Vec3f &g = inputs.mpu.gyro_rate, &a = inputs.mpu.accel,
&m = inputs.mpu.mag;
@ -86,6 +146,60 @@ struct State {
ahrs_.getPitch(), ahrs_.getRoll());
last_print = time_us;
}
comms->Lock();
UpdateLocationFromGPS(comms->location, inputs.gps);
UGV_State ugv_state = comms->ugv_state;
comms->Unlock();
switch (ugv_state) {
default:
ESP_LOGW(TAG, "unhandled state: %d", ugv_state);
// fall through
case UGV_State::STATE_IDLE:
case UGV_State::STATE_FINISHED:
outputs.left_motor = 0.0;
outputs.right_motor = 0.0;
break;
case UGV_State::STATE_AQUIRING: {
TickType_t current_tick = xTaskGetTickCount();
TickType_t ticks_since_gps = current_tick - inputs.gps.last_update;
bool not_old = ticks_since_gps <= pdMS_TO_TICKS(2000);
bool not_invalid = inputs.gps.fix_quality != io::GPS_FIX_INVALID;
outputs.left_motor = 0.0;
outputs.right_motor = 0.0;
if (not_old && not_invalid) {
comms->ugv_state = UGV_State::STATE_DRIVING;
}
break;
}
case UGV_State::STATE_DRIVING: {
LatLong current_pos = {inputs.gps.latitude, inputs.gps.longitude};
float tgt_dist = current_pos.distance_to(target);
if (tgt_dist <= MIN_DIST) {
ESP_LOGI(TAG, "Finished driving to target");
comms->ugv_state = UGV_State::STATE_FINISHED;
break;
}
float tgt_bearing = current_pos.bearing_toward(target);
float cur_bearing = ahrs_.getYaw();
float angle_delta = tgt_bearing - cur_bearing;
if (angle_delta < 180.f) angle_delta += 360.f;
if (angle_delta > 180.f) angle_delta -= 360.f;
float angle_pwr = angle_delta * ANGLE_P;
outputs.left_motor = DRIVE_POWER + angle_pwr;
outputs.right_motor = DRIVE_POWER - angle_pwr;
break;
}
case UGV_State::STATE_TEST:
outputs.left_motor = sinf(time_s * PI);
outputs.right_motor = cosf(time_s * PI);
break;
}
io->WriteOutputs(outputs);
}
};