diff --git a/e32_client/ugv.py b/e32_client/ugv.py index bc20f50..94e916f 100755 --- a/e32_client/ugv.py +++ b/e32_client/ugv.py @@ -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) diff --git a/main/messages.proto b/main/messages.proto index dfdb57a..195f9bf 100644 --- a/main/messages.proto +++ b/main/messages.proto @@ -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 { diff --git a/main/ugv_comms.cc b/main/ugv_comms.cc index ad35f10..698c9c2 100644 --- a/main/ugv_comms.cc +++ b/main/ugv_comms.cc @@ -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()); diff --git a/main/ugv_main.cc b/main/ugv_main.cc index f775db5..031f956 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -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); } };