diff --git a/CMakeLists.txt b/CMakeLists.txt index 82053f4..91402c7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,4 +18,5 @@ add_executable(bycmd_test ${BYCMD} ${TOML} ${CRC16} ${LOGC} ${TEST_MAIN}) add_library(bycmd SHARED ${BYCMD} ${TOML} ${CRC16} ${LOGC}) target_link_libraries(bycmd_test pthread) +target_link_libraries(bycmd pthread) # target_link_libraries(${test} bycmd) \ No newline at end of file diff --git a/by_cmd.c b/by_cmd.c index 18de60e..62790cd 100644 --- a/by_cmd.c +++ b/by_cmd.c @@ -15,6 +15,10 @@ uint32_t received_data[2]; uint8_t listerning_cmd; int received_flag; +pthread_mutex_t lock = PTHREAD_MUTEX_INITIALIZER; +#define LOCKAPI() pthread_mutex_lock(&lock) +#define UNLOCKAPI() pthread_mutex_unlock(&lock) + int by_cmd_init(void) { log_debug("by_cmd init"); @@ -77,9 +81,12 @@ int by_cmd_reg_listerning(uint8_t cmd, int timeout) */ void by_cmd_send_speed_x(float speed) { + LOCKAPI(); + uint8_t buff[4] = {0}; memcpy(buff, &speed, 4); by_frame_send(0x31, buff, 4); + UNLOCKAPI(); } /** @@ -89,9 +96,13 @@ void by_cmd_send_speed_x(float speed) */ void by_cmd_send_speed_y(float speed) { + LOCKAPI(); + uint8_t buff[4] = {0}; memcpy(buff, &speed, 4); by_frame_send(0x32, buff, 4); + + UNLOCKAPI(); } /** @@ -101,9 +112,13 @@ void by_cmd_send_speed_y(float speed) */ void by_cmd_send_speed_omega(float speed) { + LOCKAPI(); + uint8_t buff[4] = {0}; memcpy(buff, &speed, 4); by_frame_send(0x33, buff, 4); + + UNLOCKAPI(); } /** @@ -116,11 +131,16 @@ void by_cmd_send_speed_omega(float speed) */ int by_cmd_send_distance_x(float speed, uint32_t time) { + LOCKAPI(); + log_info("send distance_x speed:%.1f, time:%dms", speed, time * 5); + int ret = 0; uint8_t buff[8] = {0}; memcpy(buff, &speed, 4); memcpy(buff + 4, &time, 4); by_frame_send(0x34, buff, 8); - return (by_cmd_reg_listerning(0x34, 1000)); + ret = by_cmd_reg_listerning(0x34, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -133,11 +153,16 @@ int by_cmd_send_distance_x(float speed, uint32_t time) */ int by_cmd_send_distance_y(float speed, uint32_t time) { + LOCKAPI(); + log_info("send distance_y speed:%.1f, time:%dms", speed, time * 5); + int ret = 0; uint8_t buff[8] = {0}; memcpy(buff, &speed, 4); memcpy(buff + 4, &time, 4); by_frame_send(0x35, buff, 8); - return (by_cmd_reg_listerning(0x35, 1000)); + ret = by_cmd_reg_listerning(0x35, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -150,32 +175,52 @@ int by_cmd_send_distance_y(float speed, uint32_t time) */ int by_cmd_send_angle_omega(float speed, uint32_t time) { + LOCKAPI(); + log_info("send angle_omega:%.1f, time:%dms", speed, time * 5); + int ret = 0; uint8_t buff[4] = {0}; memcpy(buff, &speed, 4); memcpy(buff + 4, &time, 4); by_frame_send(0x36, buff, 8); - return (by_cmd_reg_listerning(0x36, 1000)); + ret = by_cmd_reg_listerning(0x36, 1000); + UNLOCKAPI(); + return (ret); } int by_cmd_send_reset_axis_x(void) { + LOCKAPI(); + log_info("send reset_axis_x"); + int ret = 0; uint8_t buff[4] = {0}; by_frame_send(0x41, buff, 4); - return (by_cmd_reg_listerning(0x41, 1000)); + ret = by_cmd_reg_listerning(0x41, 1000); + UNLOCKAPI(); + return (ret); } int by_cmd_send_reset_axis_z(void) { + LOCKAPI(); + log_info("send reset_axis_z"); + int ret = 0; uint8_t buff[4] = {0}; by_frame_send(0x42, buff, 4); - return (by_cmd_reg_listerning(0x42, 1000)); + ret = by_cmd_reg_listerning(0x42, 1000); + UNLOCKAPI(); + return (ret); } int by_cmd_send_reset_end_effector(void) { + LOCKAPI(); + log_info("send end_effector"); + int ret = 0; uint8_t buff[4] = {0}; by_frame_send(0x43, buff, 4); - return (by_cmd_reg_listerning(0x43, 1000)); + ret = by_cmd_reg_listerning(0x43, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -187,11 +232,16 @@ int by_cmd_send_reset_end_effector(void) */ int by_cmd_send_distance_axis_x(uint8_t speed, float distance) { + LOCKAPI(); + log_info("send distance_axis_x speed:%d, distance:%.2f", speed, distance); + int ret = 0; uint8_t buff[8] = {0}; - memcpy(buff, &speed, 1); - memcpy(buff + 1, &distance, 4); + memcpy(&buff[0], &speed, 1); + memcpy(&buff[4], &distance, 4); by_frame_send(0x44, buff, 8); - return (by_cmd_reg_listerning(0x44, 1000)); + ret = by_cmd_reg_listerning(0x44, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -203,11 +253,16 @@ int by_cmd_send_distance_axis_x(uint8_t speed, float distance) */ int by_cmd_send_distance_axis_z(uint8_t speed, float distance) { + LOCKAPI(); + log_info("send distance_axis_z speed:%d, distance:%.2f", speed, distance); + int ret = 0; uint8_t buff[8] = {0}; - memcpy(buff, &speed, 1); - memcpy(buff + 1, &distance, 4); + memcpy(&buff[0], &speed, 1); + memcpy(&buff[4], &distance, 4); by_frame_send(0x46, buff, 8); - return (by_cmd_reg_listerning(0x46, 1000)); + ret = by_cmd_reg_listerning(0x46, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -219,11 +274,16 @@ int by_cmd_send_distance_axis_z(uint8_t speed, float distance) */ int by_cmd_send_position_axis_x(uint8_t speed, float position) { + LOCKAPI(); + log_info("send position_axis_x speed:%d, distance:%.2f", speed, position); + int ret = 0; uint8_t buff[8] = {0}; - memcpy(buff, &speed, 1); - memcpy(buff + 1, &position, 4); + memcpy(&buff[0], &speed, 1); + memcpy(&buff[4], &position, 4); by_frame_send(0x47, buff, 8); - return (by_cmd_reg_listerning(0x47, 1000)); + ret = by_cmd_reg_listerning(0x47, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -235,11 +295,16 @@ int by_cmd_send_position_axis_x(uint8_t speed, float position) */ int by_cmd_send_position_axis_z(uint8_t speed, float position) { + LOCKAPI(); + log_info("send position_axis_z speed:%d, distance:%.2f", speed, position); + int ret = 0; uint8_t buff[8] = {0}; - memcpy(buff, &speed, 1); - memcpy(buff + 1, &position, 4); + memcpy(&buff[0], &speed, 1); + memcpy(&buff[4], &position, 4); by_frame_send(0x49, buff, 8); - return (by_cmd_reg_listerning(0x49, 1000)); + ret = by_cmd_reg_listerning(0x49, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -250,10 +315,15 @@ int by_cmd_send_position_axis_z(uint8_t speed, float position) */ int by_cmd_send_angle_claw_arm(float angle) { + LOCKAPI(); + log_info("send angle_claw_arm angle:%.2f", angle); + int ret = 0; uint8_t buff[4] = {0}; memcpy(buff, &angle, 4); by_frame_send(0x50, buff, 4); - return (by_cmd_reg_listerning(0x50, 1000)); + ret = by_cmd_reg_listerning(0x50, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -264,10 +334,15 @@ int by_cmd_send_angle_claw_arm(float angle) */ int by_cmd_send_angle_claw(float angle) { + LOCKAPI(); + log_info("send angle_claw angle:%.2f", angle); + int ret = 0; uint8_t buff[4] = {0}; memcpy(buff, &angle, 4); by_frame_send(0x51, buff, 4); - return (by_cmd_reg_listerning(0x51, 1000)); + ret = by_cmd_reg_listerning(0x51, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -278,10 +353,15 @@ int by_cmd_send_angle_claw(float angle) */ int by_cmd_send_angle_camera(float angle) { + LOCKAPI(); + log_info("send angle_camera angle:%.2f", angle); + int ret = 0; uint8_t buff[4] = {0}; memcpy(buff, &angle, 4); by_frame_send(0x52, buff, 4); - return (by_cmd_reg_listerning(0x52, 1000)); + ret = by_cmd_reg_listerning(0x52, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -292,10 +372,15 @@ int by_cmd_send_angle_camera(float angle) */ int by_cmd_send_angle_scoop(float angle) { + LOCKAPI(); + log_info("send angle_scoop angle:%.2f", angle); + int ret = 0; uint8_t buff[4] = {0}; memcpy(buff, &angle, 4); by_frame_send(0x53, buff, 4); - return (by_cmd_reg_listerning(0x53, 1000)); + ret = by_cmd_reg_listerning(0x53, 1000); + UNLOCKAPI(); + return (ret); } /** @@ -306,25 +391,45 @@ int by_cmd_send_angle_scoop(float angle) */ int by_cmd_send_angle_storage(float angle) { + LOCKAPI(); + log_info("send angle_storage angle:%.2f", angle); + int ret = 0; uint8_t buff[4] = {0}; memcpy(buff, &angle, 4); by_frame_send(0x54, buff, 4); - return (by_cmd_reg_listerning(0x54, 1000)); + ret = by_cmd_reg_listerning(0x54, 1000); + UNLOCKAPI(); + return (ret); } int by_cmd_send_ranging_start(void) { + LOCKAPI(); + + int ret = 0; uint8_t buff[4] = {0}; by_frame_send(0x55, buff, 4); - return (by_cmd_reg_listerning(0x55, 1000)); + ret = by_cmd_reg_listerning(0x55, 1000); + UNLOCKAPI(); + return (ret); } int by_cmd_recv_ranging_data(float *distance) { + LOCKAPI(); + + int ret = 0; uint8_t buff[4] = {0}; by_frame_send(0x56, buff, 4); - if (!by_cmd_reg_listerning(0x56, 1000)) { + ret = by_cmd_reg_listerning(0x56, 1000); + + if (!ret) { memcpy(distance, &received_data[0], 4); + } + + UNLOCKAPI(); + + if (!ret) { return 0; } else { return -1; diff --git a/by_frame.c b/by_frame.c index 02fed6e..b14e14a 100644 --- a/by_frame.c +++ b/by_frame.c @@ -146,7 +146,7 @@ int by_frame_parse(uint8_t *cmd, uint32_t *data_array) uint16_t crc_cal = crc16_check(frame_buff, BY_FRANE_LEN - 2); if (crc_val == crc_cal) { - log_info("received successful"); + log_debug("received successful"); // 丢掉当前帧头,下次解析时就不从该帧头开始 by_frame_queue_drop(&queue_recv, 1); // TODO 复制数据 @@ -154,8 +154,8 @@ int by_frame_parse(uint8_t *cmd, uint32_t *data_array) memcpy(data_array, &frame_buff[2], BY_FRANE_DATA_LEN); return 0; } else { - log_warn("receive failed"); - log_warn("cal crc 0x%04X, but got 0x%04X", crc_cal, crc_val); + log_debug("receive failed"); + log_debug("cal crc 0x%04X, but got 0x%04X", crc_cal, crc_val); // 丢掉当前帧头,下次解析时就不从该帧头开始 by_frame_queue_drop(&queue_recv, 1); return -1; diff --git a/logc/log.h b/logc/log.h index b1fae24..f2cb4b1 100644 --- a/logc/log.h +++ b/logc/log.h @@ -5,6 +5,10 @@ * under the terms of the MIT license. See `log.c` for details. */ +#if defined(__cplusplus) +extern "C" { +#endif + #ifndef LOG_H #define LOG_H @@ -28,16 +32,23 @@ typedef struct { typedef void (*log_LogFn)(log_Event *ev); typedef void (*log_LockFn)(bool lock, void *udata); -enum { LOG_TRACE, LOG_DEBUG, LOG_INFO, LOG_WARN, LOG_ERROR, LOG_FATAL }; +enum { + LOG_TRACE, + LOG_DEBUG, + LOG_INFO, + LOG_WARN, + LOG_ERROR, + LOG_FATAL +}; #define log_trace(...) log_log(LOG_TRACE, __FILE__, __LINE__, __VA_ARGS__) #define log_debug(...) log_log(LOG_DEBUG, __FILE__, __LINE__, __VA_ARGS__) -#define log_info(...) log_log(LOG_INFO, __FILE__, __LINE__, __VA_ARGS__) -#define log_warn(...) log_log(LOG_WARN, __FILE__, __LINE__, __VA_ARGS__) +#define log_info(...) log_log(LOG_INFO, __FILE__, __LINE__, __VA_ARGS__) +#define log_warn(...) log_log(LOG_WARN, __FILE__, __LINE__, __VA_ARGS__) #define log_error(...) log_log(LOG_ERROR, __FILE__, __LINE__, __VA_ARGS__) #define log_fatal(...) log_log(LOG_FATAL, __FILE__, __LINE__, __VA_ARGS__) -const char* log_level_string(int level); +const char *log_level_string(int level); void log_set_lock(log_LockFn fn, void *udata); void log_set_level(int level); void log_set_quiet(bool enable); @@ -47,3 +58,7 @@ int log_add_fp(FILE *fp, int level); void log_log(int level, const char *file, int line, const char *fmt, ...); #endif + +#if defined(__cplusplus) +} +#endif \ No newline at end of file diff --git a/test.c b/test.c index 6c81302..b5d1ea0 100644 --- a/test.c +++ b/test.c @@ -15,10 +15,24 @@ uint32_t buff_temp[20]; int main(int argc, char **argv) { + log_set_level(LOG_INFO); + if (by_cmd_init()) { log_error("Program exits abnormally"); return -1; } + by_cmd_send_distance_x(12, 100); + by_cmd_send_angle_camera(20.0); + by_cmd_send_position_axis_z(0x11, 100); + by_cmd_send_distance_axis_z(10, 20); + by_cmd_send_distance_axis_x(4, 100); + by_cmd_send_position_axis_x(4, 0); + by_cmd_send_angle_claw_arm(34); + by_cmd_send_angle_claw_arm(220); + by_cmd_send_angle_claw(27); + sleep(1); + by_cmd_send_distance_axis_x(4, -60); + return 0; }