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

Implement initial driving to target logic

drive-to-target
Alex Mikhalev 6 years ago
parent
2233eeff4e
commit
b55eb69eb8
4 changed files with 149 additions and 19 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. 3
      e32_client/ugv.py
  2. 14
      main/messages.proto
  3. 19
      main/ugv_comms.cc
  4. 124
      main/ugv_main.cc

3
e32_client/ugv.py
Unescape View File

@ -30,6 +30,7 @@ class UGVComms(E32):
msg_len = int.from_bytes(len_data, byteorder='big') msg_len = int.from_bytes(len_data, byteorder='big')
data = self.ser.read(size=msg_len) data = self.ser.read(size=msg_len)
if len(data) != msg_len: if len(data) != msg_len:
print("read bad data: ", data)
self.ser.flush() self.ser.flush()
return None return None
msg = messages.UGV_Message() msg = messages.UGV_Message()
@ -49,7 +50,7 @@ def __rx_thread_entry(ugv: UGVComms):
if __name__ == "__main__": 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, stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
timeout=2.0) timeout=2.0)
ugv = UGVComms(ser) ugv = UGVComms(ser)

14
main/messages.proto
Unescape View File

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

19
main/ugv_comms.cc
Unescape View File

@ -16,7 +16,7 @@ static const char *TAG = "ugv_comms";
CommsClass::CommsClass() CommsClass::CommsClass()
: location(), : location(),
ugv_state(messages::IDLE), ugv_state(messages::STATE_IDLE),
last_packet_tick(0), last_packet_tick(0),
last_packet_rssi(INT32_MIN), last_packet_rssi(INT32_MIN),
last_packet_snr(INT8_MIN) { last_packet_snr(INT8_MIN) {
@ -30,7 +30,7 @@ void CommsClass::Init() {
location.set_latitude(43.65); location.set_latitude(43.65);
location.set_longitude(-116.20); location.set_longitude(-116.20);
location.set_altitude(2730); location.set_altitude(2730);
ugv_state = messages::UGV_State::IDLE; ugv_state = messages::UGV_State::STATE_IDLE;
#ifdef COMMS_SX127X #ifdef COMMS_SX127X
sx127x_config_t lora_config = sx127x_config_default(); sx127x_config_t lora_config = sx127x_config_default();
@ -239,7 +239,20 @@ void CommsClass::HandlePacket(const uint8_t *data, size_t data_size) {
void CommsClass::HandleCommand(const messages::GroundCommand &command) { 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());
// 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; messages::UGV_Message ugv_message;
ugv_message.set_command_ack(command.id()); ugv_message.set_command_ack(command.id());

124
main/ugv_main.cc
Unescape View File

@ -11,6 +11,7 @@
namespace ugv { namespace ugv {
using ugv::comms::CommsClass; using ugv::comms::CommsClass;
using ugv::comms::messages::UGV_State;
using ugv::io::IOClass; using ugv::io::IOClass;
static const char *TAG = "ugv_main"; static const char *TAG = "ugv_main";
@ -20,10 +21,71 @@ SemaphoreHandle_t i2c_mutex;
} }
constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; 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); 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 { struct State {
public: public:
CommsClass * comms; CommsClass * comms;
@ -34,8 +96,9 @@ struct State {
io::Outputs outputs; io::Outputs outputs;
int64_t last_print; int64_t last_print;
Madgwick ahrs_; Madgwick ahrs_;
LatLong target;
State() { State() : target{34.069022, -118.443067} {
comms = new CommsClass(); comms = new CommsClass();
io = new IOClass(); io = new IOClass();
display = new DisplayClass(comms); display = new DisplayClass(comms);
@ -67,9 +130,6 @@ struct State {
int64_t time_us = esp_timer_get_time(); int64_t time_us = esp_timer_get_time();
float time_s = ((float)time_us) / 1e6; float time_s = ((float)time_us) / 1e6;
io->ReadInputs(inputs); 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, io::Vec3f &g = inputs.mpu.gyro_rate, &a = inputs.mpu.accel,
&m = inputs.mpu.mag; &m = inputs.mpu.mag;
@ -86,6 +146,60 @@ struct State {
ahrs_.getPitch(), ahrs_.getRoll()); ahrs_.getPitch(), ahrs_.getRoll());
last_print = time_us; 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);
} }
}; };

Write Preview
Loading…
Cancel
Save