From d368fd0c9b8489ea30617772afa5c041bfcb68a8 Mon Sep 17 00:00:00 2001 From: e5l Date: Mon, 22 Aug 2016 14:55:27 +0300 Subject: [PATCH] car_fmw: sonar support --- car_cfmw/src/main.c | 8 +- car_fmw/src/Control.kt | 34 ++- car_fmw/src/ProtoRW.kt | 19 +- car_fmw/src/include/Sonar.kt | 11 + car_fmw/src/main.kt | 1 + car_fmw/src/proto/Sonar.proto | 10 + car_fmw/src/proto/Task.proto | 1 + car_fmw/src/proto_src/Sonar.kt | 478 +++++++++++++++++++++++++++++++++ car_fmw/src/proto_src/Task.kt | 4 +- 9 files changed, 550 insertions(+), 16 deletions(-) create mode 100644 car_fmw/src/include/Sonar.kt create mode 100644 car_fmw/src/proto/Sonar.proto create mode 100644 car_fmw/src/proto_src/Sonar.kt diff --git a/car_cfmw/src/main.c b/car_cfmw/src/main.c index 870f2359e94..081a0aeb0f1 100644 --- a/car_cfmw/src/main.c +++ b/car_cfmw/src/main.c @@ -95,7 +95,11 @@ int main(void) car_conn_init(); car_sonar_init(); - sonar_prog_rotate(); - //sonar_prog_dist_to_conn(); + car_sonar_get_dist(0); + car_time_wait(3000); + car_sonar_get_dist(180); + car_time_wait(3000); + /* sonar_prog_rotate(); */ + sonar_prog_dist_to_conn(); // Also we can run usart test from here } diff --git a/car_fmw/src/Control.kt b/car_fmw/src/Control.kt index 9080c7fd614..dd57ec69ced 100644 --- a/car_fmw/src/Control.kt +++ b/car_fmw/src/Control.kt @@ -17,7 +17,7 @@ object Control { } } - fun executeCommand() { + private fun executeCommand() { val task = Reader.readTask() Leds.blink() Time.wait(BLINK_DURATION) @@ -25,13 +25,14 @@ object Control { when (task.type.id) { TaskRequest.Type.DEBUG.id -> debugTask() TaskRequest.Type.ROUTE.id -> routeTask() + TaskRequest.Type.SONAR.id -> sonarTask() } Leds.blink() Time.wait(BLINK_DURATION) } - fun debugTask() { + private fun debugTask() { val request = Reader.readDebug() when (request.type.id) { @@ -39,16 +40,16 @@ object Control { } } - fun routeTask() { + private fun routeTask() { val route = Reader.readRoute() val times = route.times val directions = route.directions - var j = 0 + var i = 0 - while (j < times.size) { - val time = times[j] - val direction = directions[j] + while (i < times.size) { + val time = times[i] + val direction = directions[i] when (direction) { RouteType.FORWARD.id -> Engine.forward() @@ -59,14 +60,29 @@ object Control { Time.wait(time) Engine.stop() - j++ + i++ } val response = RouteResponse.BuilderRouteResponse(0).build() Writer.writeRoute(response) } - fun sendMemoryStats() { + private fun sonarTask() { + val request = Reader.readSonar() + val size = request.angles.size + val distances = IntArray(size) + + var i = 0 + while (i < size) { + distances[i] = Sonar.getDistance(request.angles[i]) + i++ + } + + val response = SonarResponse.BuilderSonarResponse(distances).build() + Writer.writeSonar(response) + } + + private fun sendMemoryStats() { val stats = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats( DebugInfo.getDynamicHeapTail(), DebugInfo.getStaticHeapTail(), diff --git a/car_fmw/src/ProtoRW.kt b/car_fmw/src/ProtoRW.kt index 110e36d5575..88315ad5a78 100644 --- a/car_fmw/src/ProtoRW.kt +++ b/car_fmw/src/ProtoRW.kt @@ -14,7 +14,12 @@ object Reader { return DebugRequest.BuilderDebugRequest(DebugRequest.Type.MEMORY_STATS).parseFrom(stream).build() } - fun getInputStream(): CodedInputStream { + fun readSonar(): SonarRequest { + val stream = getInputStream() + return SonarRequest.BuilderSonarRequest(IntArray(0)).parseFrom(stream).build() + } + + private fun getInputStream(): CodedInputStream { val buffer = Connection.receiveByteArray() return CodedInputStream(buffer) } @@ -22,18 +27,24 @@ object Reader { object Writer { fun writeRoute(route: RouteResponse) { - val stream = makeOutputStream(route.getSizeNoTag()) + val stream = getOutputStream(route.getSizeNoTag()) route.writeTo(stream) Connection.sendByteArray(stream.buffer) } fun writeMemoryStats(stats: DebugResponseMemoryStats) { - val stream = makeOutputStream(stats.getSizeNoTag()) + val stream = getOutputStream(stats.getSizeNoTag()) stats.writeTo(stream) Connection.sendByteArray(stream.buffer) } - fun makeOutputStream(size: Int): CodedOutputStream { + fun writeSonar(response: SonarResponse) { + val stream = getOutputStream(response.getSizeNoTag()) + response.writeTo(stream) + Connection.sendByteArray(stream.buffer) + } + + private fun getOutputStream(size: Int): CodedOutputStream { val buffer = ByteArray(size) return CodedOutputStream(buffer) } diff --git a/car_fmw/src/include/Sonar.kt b/car_fmw/src/include/Sonar.kt new file mode 100644 index 00000000000..6b19de5a128 --- /dev/null +++ b/car_fmw/src/include/Sonar.kt @@ -0,0 +1,11 @@ + +external fun car_sonar_init() +external fun car_sonar_get_dist(degree: Byte): Short + +object Sonar { + fun init() { + car_sonar_init() + } + + fun getDistance(degree: Int): Int = car_sonar_get_dist(degree.toByte()).toInt() +} \ No newline at end of file diff --git a/car_fmw/src/main.kt b/car_fmw/src/main.kt index ee2706f8482..d3cb39ff58a 100644 --- a/car_fmw/src/main.kt +++ b/car_fmw/src/main.kt @@ -4,6 +4,7 @@ fun main() { Time.init() Engine.init() Leds.init() + Sonar.init() Connection.init() /* car task */ diff --git a/car_fmw/src/proto/Sonar.proto b/car_fmw/src/proto/Sonar.proto new file mode 100644 index 00000000000..086804d029e --- /dev/null +++ b/car_fmw/src/proto/Sonar.proto @@ -0,0 +1,10 @@ +syntax = "proto3"; +package carkot; + +message SonarRequest { + repeated int32 angles = 1; +} + +message SonarResponse { + repeated int32 distances = 1; +} diff --git a/car_fmw/src/proto/Task.proto b/car_fmw/src/proto/Task.proto index 11fe5145fec..b61e72a90b0 100644 --- a/car_fmw/src/proto/Task.proto +++ b/car_fmw/src/proto/Task.proto @@ -8,5 +8,6 @@ message TaskRequest { enum Type { DEBUG = 0; ROUTE = 1; + SONAR = 2; } } diff --git a/car_fmw/src/proto_src/Sonar.kt b/car_fmw/src/proto_src/Sonar.kt new file mode 100644 index 00000000000..d06ec94ac4f --- /dev/null +++ b/car_fmw/src/proto_src/Sonar.kt @@ -0,0 +1,478 @@ +class SonarRequest private constructor (var angles: IntArray) { + //========== Properties =========== + //repeated int32 angles = 1 + + var errorCode: Int = 0 + + //========== Serialization methods =========== + fun writeTo (output: CodedOutputStream) { + //repeated int32 angles = 1 + if (angles.size > 0) { + output.writeTag(1, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (angles.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < angles.size) { + arraySize += WireFormat.getInt32SizeNoTag(angles[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < angles.size) { + output.writeInt32NoTag (angles[i]) + i += 1 + } + } while(false) + } + + } + + fun mergeWith (other: SonarRequest) { + angles = angles.plus((other.angles)) + this.errorCode = other.errorCode + } + + fun mergeFromWithSize (input: CodedInputStream, expectedSize: Int) { + val builder = SonarRequest.BuilderSonarRequest(IntArray(0)) + mergeWith(builder.parseFromWithSize(input, expectedSize).build()) + } + + fun mergeFrom (input: CodedInputStream) { + val builder = SonarRequest.BuilderSonarRequest(IntArray(0)) + mergeWith(builder.parseFrom(input).build()) + } + + //========== Size-related methods =========== + fun getSize(fieldNumber: Int): Int { + var size = 0 + if (angles.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < angles.size) { + arraySize += WireFormat.getInt32SizeNoTag(angles[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED) + return size + } + + fun getSizeNoTag(): Int { + var size = 0 + if (angles.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < angles.size) { + arraySize += WireFormat.getInt32SizeNoTag(angles[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + return size + } + + //========== Builder =========== + class BuilderSonarRequest constructor (var angles: IntArray) { + //========== Properties =========== + //repeated int32 angles = 1 + fun setAngles(value: IntArray): SonarRequest.BuilderSonarRequest { + angles = value + return this + } + fun setanglesByIndex(index: Int, value: Int): SonarRequest.BuilderSonarRequest { + angles[index] = value + return this + } + + var errorCode: Int = 0 + + //========== Serialization methods =========== + fun writeTo (output: CodedOutputStream) { + //repeated int32 angles = 1 + if (angles.size > 0) { + output.writeTag(1, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (angles.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < angles.size) { + arraySize += WireFormat.getInt32SizeNoTag(angles[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < angles.size) { + output.writeInt32NoTag (angles[i]) + i += 1 + } + } while(false) + } + + } + + //========== Mutating methods =========== + fun build(): SonarRequest { + val res = SonarRequest(angles) + res.errorCode = errorCode + return res + } + + fun parseFieldFrom(input: CodedInputStream): Boolean { + if (input.isAtEnd()) { return false } + val tag = input.readInt32NoTag() + if (tag == 0) { return false } + val fieldNumber = WireFormat.getTagFieldNumber(tag) + val wireType = WireFormat.getTagWireType(tag) + when(fieldNumber) { + 1 -> { + if (wireType.id != WireType.LENGTH_DELIMITED.id) { + errorCode = 1 + return false + } + val expectedByteSize = input.readInt32NoTag() + var readSize = 0 + var arraySize = 0 + input.mark() + do { + var i = 0 + while(readSize < expectedByteSize) { + var tmp = 0 + tmp = input.readInt32NoTag() + arraySize += 1 + readSize += WireFormat.getInt32SizeNoTag(tmp) + } + } while (false) + var newArray = IntArray(arraySize) + input.reset() + do { + var i = 0 + while(i < arraySize) { + newArray[i] = input.readInt32NoTag() + i += 1} + angles = newArray + } while (false) + } + else -> errorCode = 4 + } + return true + } + + fun parseFromWithSize(input: CodedInputStream, expectedSize: Int): SonarRequest.BuilderSonarRequest { + while(getSizeNoTag() < expectedSize) { + parseFieldFrom(input) + } + if (getSizeNoTag() > expectedSize) { errorCode = 2 } + return this + } + + fun parseFrom(input: CodedInputStream): SonarRequest.BuilderSonarRequest { + while(parseFieldFrom(input)) {} + return this + } + + //========== Size-related methods =========== + fun getSize(fieldNumber: Int): Int { + var size = 0 + if (angles.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < angles.size) { + arraySize += WireFormat.getInt32SizeNoTag(angles[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED) + return size + } + + fun getSizeNoTag(): Int { + var size = 0 + if (angles.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < angles.size) { + arraySize += WireFormat.getInt32SizeNoTag(angles[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + return size + } + + } + +} + + +class SonarResponse private constructor (var distances: IntArray) { + //========== Properties =========== + //repeated int32 distances = 1 + + var errorCode: Int = 0 + + //========== Serialization methods =========== + fun writeTo (output: CodedOutputStream) { + //repeated int32 distances = 1 + if (distances.size > 0) { + output.writeTag(1, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (distances.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < distances.size) { + output.writeInt32NoTag (distances[i]) + i += 1 + } + } while(false) + } + + } + + fun mergeWith (other: SonarResponse) { + distances = distances.plus((other.distances)) + this.errorCode = other.errorCode + } + + fun mergeFromWithSize (input: CodedInputStream, expectedSize: Int) { + val builder = SonarResponse.BuilderSonarResponse(IntArray(0)) + mergeWith(builder.parseFromWithSize(input, expectedSize).build()) + } + + fun mergeFrom (input: CodedInputStream) { + val builder = SonarResponse.BuilderSonarResponse(IntArray(0)) + mergeWith(builder.parseFrom(input).build()) + } + + //========== Size-related methods =========== + fun getSize(fieldNumber: Int): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED) + return size + } + + fun getSizeNoTag(): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + return size + } + + //========== Builder =========== + class BuilderSonarResponse constructor (var distances: IntArray) { + //========== Properties =========== + //repeated int32 distances = 1 + fun setDistances(value: IntArray): SonarResponse.BuilderSonarResponse { + distances = value + return this + } + fun setdistancesByIndex(index: Int, value: Int): SonarResponse.BuilderSonarResponse { + distances[index] = value + return this + } + + var errorCode: Int = 0 + + //========== Serialization methods =========== + fun writeTo (output: CodedOutputStream) { + //repeated int32 distances = 1 + if (distances.size > 0) { + output.writeTag(1, WireType.LENGTH_DELIMITED) + var arrayByteSize = 0 + + if (distances.size != 0) { + do { + var arraySize = 0 + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + arrayByteSize += arraySize + } while(false) + } + output.writeInt32NoTag(arrayByteSize) + + do { + var i = 0 + while (i < distances.size) { + output.writeInt32NoTag (distances[i]) + i += 1 + } + } while(false) + } + + } + + //========== Mutating methods =========== + fun build(): SonarResponse { + val res = SonarResponse(distances) + res.errorCode = errorCode + return res + } + + fun parseFieldFrom(input: CodedInputStream): Boolean { + if (input.isAtEnd()) { return false } + val tag = input.readInt32NoTag() + if (tag == 0) { return false } + val fieldNumber = WireFormat.getTagFieldNumber(tag) + val wireType = WireFormat.getTagWireType(tag) + when(fieldNumber) { + 1 -> { + if (wireType.id != WireType.LENGTH_DELIMITED.id) { + errorCode = 1 + return false + } + val expectedByteSize = input.readInt32NoTag() + var readSize = 0 + var arraySize = 0 + input.mark() + do { + var i = 0 + while(readSize < expectedByteSize) { + var tmp = 0 + tmp = input.readInt32NoTag() + arraySize += 1 + readSize += WireFormat.getInt32SizeNoTag(tmp) + } + } while (false) + var newArray = IntArray(arraySize) + input.reset() + do { + var i = 0 + while(i < arraySize) { + newArray[i] = input.readInt32NoTag() + i += 1} + distances = newArray + } while (false) + } + else -> errorCode = 4 + } + return true + } + + fun parseFromWithSize(input: CodedInputStream, expectedSize: Int): SonarResponse.BuilderSonarResponse { + while(getSizeNoTag() < expectedSize) { + parseFieldFrom(input) + } + if (getSizeNoTag() > expectedSize) { errorCode = 2 } + return this + } + + fun parseFrom(input: CodedInputStream): SonarResponse.BuilderSonarResponse { + while(parseFieldFrom(input)) {} + return this + } + + //========== Size-related methods =========== + fun getSize(fieldNumber: Int): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + size += WireFormat.getVarint32Size(size) + WireFormat.getTagSize(fieldNumber, WireType.LENGTH_DELIMITED) + return size + } + + fun getSizeNoTag(): Int { + var size = 0 + if (distances.size != 0) { + do { + var arraySize = 0 + size += WireFormat.getTagSize(1, WireType.LENGTH_DELIMITED) + var i = 0 + while (i < distances.size) { + arraySize += WireFormat.getInt32SizeNoTag(distances[i]) + i += 1 + } + size += arraySize + size += WireFormat.getInt32SizeNoTag(arraySize) + } while(false) + } + return size + } + + } + +} + + diff --git a/car_fmw/src/proto_src/Task.kt b/car_fmw/src/proto_src/Task.kt index a3cb6d830bf..d86b6efa0e0 100644 --- a/car_fmw/src/proto_src/Task.kt +++ b/car_fmw/src/proto_src/Task.kt @@ -8,13 +8,15 @@ class TaskRequest private constructor (var type: TaskRequest.Type) { enum class Type(val id: Int) { DEBUG (0), ROUTE (1), - Unexpected(2); + SONAR (2), + Unexpected(3); companion object { fun fromIntToType (ord: Int): Type { return when (ord) { 0 -> Type.DEBUG 1 -> Type.ROUTE + 2 -> Type.SONAR else -> Unexpected } }