From 728cb18834ec9c841701339492e3629d8adf2fab Mon Sep 17 00:00:00 2001 From: e5l Date: Wed, 31 Aug 2016 09:35:55 +0300 Subject: [PATCH] car_fmw, car_srv, server: debug statistic for sonar --- car_fmw/src/Control.kt | 12 +++ car_fmw/src/ProtoRW.kt | 6 ++ car_fmw/src/include/DebugInfo.kt | 14 +++- car_fmw/src/include/Sonar.kt | 2 +- car_hw/src/car_sonar.c | 48 +++++++++++- car_hw/src/include/car_sonar.h | 6 ++ car_srv/kotlinSrv/src/Main.kt | 2 + .../src/net/server/handlers/debug/Memory.kt | 19 ++--- .../src/net/server/handlers/debug/Sonar.kt | 31 ++++++++ .../protofiles_sources/server_car/debug.proto | 8 ++ server/src/main/java/DebugClInterface.kt | 76 ++++++++----------- .../main/java/RoomScanner/CarController.kt | 2 +- server/src/main/java/car/client/CarClient.kt | 1 + 13 files changed, 164 insertions(+), 63 deletions(-) create mode 100644 car_srv/kotlinSrv/src/net/server/handlers/debug/Sonar.kt diff --git a/car_fmw/src/Control.kt b/car_fmw/src/Control.kt index 6189e0b639e..84b35561796 100644 --- a/car_fmw/src/Control.kt +++ b/car_fmw/src/Control.kt @@ -25,6 +25,7 @@ object Control { when (request.type.id) { DebugRequest.Type.MEMORY_STATS.id -> sendMemoryStats() + DebugRequest.Type.SONAR_STATS.id -> sendSonarStats() } } @@ -129,4 +130,15 @@ object Control { Writer.writeMemoryStats(stats) } + + private fun sendSonarStats() { + val stats = DebugResponseSonarStats.BuilderDebugResponseSonarStats( + DebugSonarInfo.getMeasurementCount(), + DebugSonarInfo.getMeasurementFailedChecksum(), + DebugSonarInfo.getMeasurementFailedDistance(), + DebugSonarInfo.getMeasurementFailedCommand() + ).build() + + Writer.writeSonarStats(stats) + } } diff --git a/car_fmw/src/ProtoRW.kt b/car_fmw/src/ProtoRW.kt index bbd4980986e..bb3afafbff7 100644 --- a/car_fmw/src/ProtoRW.kt +++ b/car_fmw/src/ProtoRW.kt @@ -48,6 +48,12 @@ object Writer { Connection.sendByteArray(stream.buffer) } + fun writeSonarStats(stats: DebugResponseSonarStats) { + val stream = getOutputStream(stats.getSizeNoTag()) + stats.writeTo(stream) + Connection.sendByteArray(stream.buffer) + } + fun writeSonar(response: SonarResponse) { val stream = getOutputStream(response.getSizeNoTag()) response.writeTo(stream) diff --git a/car_fmw/src/include/DebugInfo.kt b/car_fmw/src/include/DebugInfo.kt index f4c2fa47144..ec364978be0 100644 --- a/car_fmw/src/include/DebugInfo.kt +++ b/car_fmw/src/include/DebugInfo.kt @@ -9,4 +9,16 @@ object DebugInfo { fun getStaticHeapTail(): Int = static_heap_tail() fun getDynamicHeapMaxSize(): Int = dynamic_heap_max_bytes() fun getDynamicHeapTotalBytes(): Int = dynamic_heap_total() -} \ No newline at end of file +} + +external fun car_sonar_get_measurement_total(): Int +external fun car_sonar_get_measurement_failed_checksum(): Int +external fun car_sonar_get_measurement_failed_distance(): Int +external fun car_sonar_get_measurement_failed_command(): Int + +object DebugSonarInfo { + fun getMeasurementCount(): Int = car_sonar_get_measurement_total() + fun getMeasurementFailedChecksum(): Int = car_sonar_get_measurement_failed_checksum() + fun getMeasurementFailedDistance(): Int = car_sonar_get_measurement_failed_distance() + fun getMeasurementFailedCommand(): Int = car_sonar_get_measurement_failed_command() +} diff --git a/car_fmw/src/include/Sonar.kt b/car_fmw/src/include/Sonar.kt index 847542b0d3e..c7d99ef8193 100644 --- a/car_fmw/src/include/Sonar.kt +++ b/car_fmw/src/include/Sonar.kt @@ -72,4 +72,4 @@ object Sonar { fun calibrate() { getDistance(180) } -} \ No newline at end of file +} diff --git a/car_hw/src/car_sonar.c b/car_hw/src/car_sonar.c index 88ffb9ddbc6..9bb854f3b78 100644 --- a/car_hw/src/car_sonar.c +++ b/car_hw/src/car_sonar.c @@ -11,6 +11,11 @@ #define BACK_ANGLE 40 #define MAX_DEGREE 180 +uint32_t measurement_count = 0; +uint32_t measurement_failed_checksum = 0; +uint32_t measurement_failed_distance = 0; +uint32_t measurement_failed_command = 0; + static uint8_t sonar_degree(uint8_t degree) { if (degree > MAX_DEGREE) @@ -40,16 +45,51 @@ static void sonar_snd_dist_req(uint8_t rot_degree) static uint8_t sonar_resp[4]; static uint16_t sonar_wait_dist_resp(void) { + measurement_count++; + size_t i = 0; uint8_t sum = 0; + uint32_t distance = 0; usart_rcv_data(USART3_ID, sonar_resp, 4); sum = sonar_resp[0] + sonar_resp[1] + sonar_resp[2]; - if ((sonar_resp[0] != SONAR_CMD_GET_DIST) - || (sum != sonar_resp[3])) - return CAR_SONAR_DIST_ERR; - return ((uint16_t)sonar_resp[1] << 8) + sonar_resp[2]; + if (sonar_resp[0] != SONAR_CMD_GET_DIST) { + measurement_failed_command++; + return CAR_SONAR_DIST_ERR; + } + + if (sum != sonar_resp[3]) { + measurement_failed_checksum++; + return CAR_SONAR_DIST_ERR; + } + + distance = ((uint16_t)sonar_resp[1] << 8) + sonar_resp[2]; + if (distance == -1) { + measurement_failed_distance++; + } + + return distance; +} + +uint32_t car_sonar_get_measurement_total() +{ + return measurement_count; +} + +uint32_t car_sonar_get_measurement_failed_checksum() +{ + return measurement_failed_checksum; +} + +uint32_t car_sonar_get_measurement_failed_distance() +{ + return measurement_failed_distance; +} + +uint32_t car_sonar_get_measurement_failed_command() +{ + return measurement_failed_command; } uint16_t car_sonar_get_dist(uint8_t rot_degree) diff --git a/car_hw/src/include/car_sonar.h b/car_hw/src/include/car_sonar.h index aa192e11d73..f5a3d08faae 100644 --- a/car_hw/src/include/car_sonar.h +++ b/car_hw/src/include/car_sonar.h @@ -22,3 +22,9 @@ void car_sonar_init(void); // returns CAR_SONAR_DIST_ERR on error // else distance in centimeters uint16_t car_sonar_get_dist(uint8_t rot_degree); + +uint32_t car_sonar_get_measurement_total(); +uint32_t car_sonar_get_measurement_failed_checksum(); +uint32_t car_sonar_get_measurement_failed_distance(); +uint32_t car_sonar_get_measurement_failed_command(); + diff --git a/car_srv/kotlinSrv/src/Main.kt b/car_srv/kotlinSrv/src/Main.kt index 3b233abcfc5..e43e2279989 100644 --- a/car_srv/kotlinSrv/src/Main.kt +++ b/car_srv/kotlinSrv/src/Main.kt @@ -4,6 +4,7 @@ import control.emulator.Rng import net.Client import net.server.handlers.AbstractHandler import net.server.handlers.debug.Memory +import net.server.handlers.debug.Sonar import net.server.handlers.flash.LoadBin import net.server.handlers.main.* import net.server.handlers.rc.Control @@ -61,6 +62,7 @@ fun main(args: Array) { handlers.put("/routeMetric", SetRouteMetric(carController)) handlers.put("/getLocation", GetLocation()) handlers.put("/debug/memory", Memory()) + handlers.put("/debug/sonar", Sonar()) Client.instance.connectToServer(config.getCarIp(), carServerPort) diff --git a/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt b/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt index 35864dfbfce..7488e349f82 100644 --- a/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt +++ b/car_srv/kotlinSrv/src/net/server/handlers/debug/Memory.kt @@ -8,28 +8,23 @@ import encodeProtoBuf import mcTransport import net.server.handlers.AbstractHandler -class Memory : AbstractHandler { - - val fromServerObjectBuilder: DebugRequest.BuilderDebugRequest - val toServerObjectBuilder: DebugResponseMemoryStats.BuilderDebugResponseMemoryStats - - constructor() : super() { - fromServerObjectBuilder = DebugRequest.BuilderDebugRequest(DebugRequest.Type.MEMORY_STATS) - toServerObjectBuilder = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0) - } +class Memory() : AbstractHandler() { + val request = DebugRequest.BuilderDebugRequest(DebugRequest.Type.MEMORY_STATS) + val response = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0) override fun getBytesResponse(data: ByteArray, callback: (ByteArray) -> Unit) { - if (!McState.instance.isConnected()) { println("mc is disconnected!") - val responseMessage = toServerObjectBuilder.build() + val responseMessage = response.build() callback.invoke(encodeProtoBuf(responseMessage)) return } + mcTransport.setCallBack { bytes -> callback.invoke(bytes) } - val message = fromServerObjectBuilder.build() + + val message = request.build() message.mergeFrom(CodedInputStream(data)) mcTransport.sendProtoBuf(message) } diff --git a/car_srv/kotlinSrv/src/net/server/handlers/debug/Sonar.kt b/car_srv/kotlinSrv/src/net/server/handlers/debug/Sonar.kt new file mode 100644 index 00000000000..c2b8dd250a2 --- /dev/null +++ b/car_srv/kotlinSrv/src/net/server/handlers/debug/Sonar.kt @@ -0,0 +1,31 @@ +package net.server.handlers.debug + +import CodedInputStream +import DebugRequest +import DebugResponseSonarStats +import McState +import encodeProtoBuf +import mcTransport +import net.server.handlers.AbstractHandler + +class Sonar : AbstractHandler() { + val request = DebugRequest.BuilderDebugRequest(DebugRequest.Type.SONAR_STATS) + val response = DebugResponseSonarStats.BuilderDebugResponseSonarStats(0, 0, 0, 0) + + override fun getBytesResponse(data: ByteArray, callback: (ByteArray) -> Unit) { + if (!McState.instance.isConnected()) { + println("mc is disconnected!") + val responseMessage = response.build() + callback.invoke(encodeProtoBuf(responseMessage)) + return + } + + mcTransport.setCallBack { bytes -> + callback.invoke(bytes) + } + + val message = request.build() + message.mergeFrom(CodedInputStream(data)) + mcTransport.sendProtoBuf(message) + } +} diff --git a/proto/protofiles_sources/server_car/debug.proto b/proto/protofiles_sources/server_car/debug.proto index 0559c8d3cd6..1b2aa564825 100644 --- a/proto/protofiles_sources/server_car/debug.proto +++ b/proto/protofiles_sources/server_car/debug.proto @@ -6,6 +6,7 @@ message DebugRequest { enum Type { MEMORY_STATS = 0; + SONAR_STATS = 1; } } @@ -15,3 +16,10 @@ message DebugResponseMemoryStats { int32 heapDynamicMaxBytes = 3; int32 heapDynamicTotalBytes = 4; } + +message DebugResponseSonarStats { + int32 measurementCount = 1; + int32 measurementFailedChecksum = 2; + int32 measurementFailedDistance = 3; + int32 measurementFailedCommand = 4; +} diff --git a/server/src/main/java/DebugClInterface.kt b/server/src/main/java/DebugClInterface.kt index fac79c3bdf0..00ce61aace5 100644 --- a/server/src/main/java/DebugClInterface.kt +++ b/server/src/main/java/DebugClInterface.kt @@ -8,6 +8,7 @@ import io.netty.buffer.Unpooled import io.netty.handler.codec.http.* import objects.Car import objects.Environment +import java.rmi.UnexpectedException import java.util.concurrent.Exchanger object DebugClInterface { @@ -92,7 +93,7 @@ object DebugClInterface { CarClient.serialize(request.getSizeNoTag(), { request.writeTo(it) }) ).get().responseBodyAsBytes - val distances = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0)).parseFrom(CodedInputStream(responseData)).distances + val distances = SonarExploreAngleResponse.BuilderSonarExploreAngleResponse(IntArray(0)).parseFrom(CodedInputStream(responseData)).build().distances println("Received distances: [${distances.joinToString()}]") } @@ -117,51 +118,38 @@ object DebugClInterface { private fun executeDebugInfoCommand(readString: String) { val params = readString.split(" ") - if (params.size != 3) { - println("incorrect args of command debug info.") - println(helpString) - return + val car = Environment.map[params[1].toInt()]!! + val type = DebugRequest.Type.fromIntToType(params[2].toInt()) + + val request = DebugRequest.BuilderDebugRequest(type).build() + val requestType = when (type) { + DebugRequest.Type.MEMORY_STATS -> CarClient.Request.DEBUG_MEMORY + DebugRequest.Type.SONAR_STATS -> CarClient.Request.DEBUG_SONAR + else -> throw UnexpectedException(type.toString()) } - val id: Int - try { - id = params[1].toInt() - } catch (e: NumberFormatException) { - e.printStackTrace() - println("error in converting id to int type") - println(helpString) - return - } - val car: Car? = - synchronized(Environment, { - Environment.map[id] - }) - if (car == null) { - println("car with id=$id not found") - return - } - val paramType = params[2]//maybe string or int - val type: DebugRequest.Type - try { - type = DebugRequest.Type.fromIntToType(paramType.toInt()) - } catch (e: NumberFormatException) { - try { - type = DebugRequest.Type.valueOf(paramType) - } catch (e: IllegalArgumentException) { - type = DebugRequest.Type.Unexpected + + val responseData = CarClient.sendRequest( + car, + requestType, + CarClient.serialize(request.getSizeNoTag(), { request.writeTo(it) }) + ).get().responseBodyAsBytes + + when (type) { + DebugRequest.Type.MEMORY_STATS -> { + val data = DebugResponseMemoryStats.BuilderDebugResponseMemoryStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build() + println("Heap static tail: ${data.heapStaticTail}") + println("Heap dynamic tail: ${data.heapDynamicTail}") + println("Heap dynamic max size: ${data.heapDynamicMaxBytes}") + println("Heap dynamic total size: ${data.heapDynamicTotalBytes}") } - } - if (type == DebugRequest.Type.Unexpected) { - println("type with name/id $paramType not found") - return - } - val requestObject = DebugRequest.BuilderDebugRequest(type).build() - val bytes = ByteArray(requestObject.getSizeNoTag()) - requestObject.writeTo(CodedOutputStream(bytes)) - val request = getDefaultHttpRequest(car.host, debugMemoryUrl, bytes) - try { - Client.sendRequest(request, car.host, car.port, getRequestOptionId(car.uid)) - } catch (e: InactiveCarException) { - println("this car is inactive") + DebugRequest.Type.SONAR_STATS -> { + val data = DebugResponseSonarStats.BuilderDebugResponseSonarStats(0, 0, 0, 0).parseFrom(CodedInputStream(responseData)).build() + println("Sonar measurement total: ${data.measurementCount}") + println("Failed checksums: ${data.measurementFailedChecksum}") + println("Failed command: ${data.measurementFailedCommand}") + println("Failed distance: ${data.measurementFailedDistance}") + } + else -> throw UnexpectedException(type.toString()) } } diff --git a/server/src/main/java/RoomScanner/CarController.kt b/server/src/main/java/RoomScanner/CarController.kt index 5832fefd0e8..5c7f269643c 100644 --- a/server/src/main/java/RoomScanner/CarController.kt +++ b/server/src/main/java/RoomScanner/CarController.kt @@ -38,7 +38,7 @@ class CarController(var car: Car) { } fun scan(angles: IntArray): List> { - val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 1 }), 10, SonarRequest.Smoothing.MEDIAN).build() + val request = SonarRequest.BuilderSonarRequest(angles, IntArray(angles.size, { 5 }), 3, SonarRequest.Smoothing.MEDIAN).build() val data = CarClient.serialize(request.getSizeNoTag(), { i -> request.writeTo(i) }) val response = CarClient.sendRequest(car, CarClient.Request.SONAR, data).get().responseBodyAsBytes diff --git a/server/src/main/java/car/client/CarClient.kt b/server/src/main/java/car/client/CarClient.kt index 624e54ee5e9..46004285506 100644 --- a/server/src/main/java/car/client/CarClient.kt +++ b/server/src/main/java/car/client/CarClient.kt @@ -13,6 +13,7 @@ object CarClient { ROUTE_METRIC("routeMetric"), SONAR("sonar"), DEBUG_MEMORY("debug/memory"), + DEBUG_SONAR("debug/sonar"), EXPLORE_ANGLE("sonarExplore"); }