From d2463f759bf25bd8d2ee7d8531784fe12b42558c Mon Sep 17 00:00:00 2001 From: Edward Emelianov Date: Thu, 3 Sep 2026 16:22:24 +0300 Subject: [PATCH] some fixes --- BTA_dome_modbus/bta_shdata.c | 16 ++-- BTA_dome_modbus/bta_shdata.h | 141 +++++++++++++++----------------- BTA_dome_modbus/client.c | 4 +- BTA_dome_modbus/esq770.h | 5 +- BTA_dome_modbus/handlers_list.h | 1 + BTA_dome_modbus/motors.c | 82 ++++++++++++++----- BTA_dome_modbus/motors.h | 8 +- BTA_dome_modbus/server.c | 9 +- 8 files changed, 154 insertions(+), 112 deletions(-) diff --git a/BTA_dome_modbus/bta_shdata.c b/BTA_dome_modbus/bta_shdata.c index 79135a1..fb30a35 100644 --- a/BTA_dome_modbus/bta_shdata.c +++ b/BTA_dome_modbus/bta_shdata.c @@ -1,6 +1,10 @@ // (C) V.S. Shergin, SAO RAS #include #include +#include +#include +#include + #include "bta_shdata.h" #pragma pack(push, 4) @@ -15,16 +19,6 @@ struct CMD_Queue ucmd = {{"Ucmd"}, 0200,0,-1,0}; static char msg[MSGLEN]; #define WARN(...) warn(__VA_ARGS__) #define PERR(...) do{snprintf(msg, MSGLEN, __VA_ARGS__); perror(msg);} while(0) -#ifdef EBUG - #define FNAME() fprintf(stderr, "\n%s (%s, line %d)\n", __func__, __FILE__, __LINE__) - #define DBG(...) do{fprintf(stderr, "%s (%s, line %d): ", __func__, __FILE__, __LINE__); \ - fprintf(stderr, __VA_ARGS__); \ - fprintf(stderr, "\n");} while(0) -#else - #define FNAME() do{}while(0) - #define DBG(...) do{}while(0) -#endif //EBUG - #ifndef BTA_MODULE volatile struct BTA_Data *sdt; @@ -141,7 +135,7 @@ int get_shm_block(volatile struct SHM_Block *sb, int server) { PERR("Can't prevents swapping of shared memory segment '%s'",sb->key.name); return 0; } - DBG("Create & attach shared memory segment '%s' %dbytes", sb->key.name, sb->size); + fprintf(stderr, "Create & attach shared memory segment '%s' %dbytes", sb->key.name, sb->size); sb->side = server; if(sb->init != NULL) sb->init(); diff --git a/BTA_dome_modbus/bta_shdata.h b/BTA_dome_modbus/bta_shdata.h index 8e715d7..4d11bec 100644 --- a/BTA_dome_modbus/bta_shdata.h +++ b/BTA_dome_modbus/bta_shdata.h @@ -1,17 +1,11 @@ // (C) V.S. Shergin, SAO RAS #pragma once -#ifndef __BTA_SHDATA_H__ -#define __BTA_SHDATA_H__ -#include -#include #include #include -#include #include #include #include -#include #pragma pack(push, 4) /* @@ -41,7 +35,7 @@ extern volatile struct SHM_Block sdat; */ struct CMD_Queue { union { - char name[5]; // queue key + char name[5]; // queue key key_t code; } key; int32_t mode; // access mode (rwxrwxrwx) @@ -318,7 +312,7 @@ enum{ ,Balance =0x10// balancing }; // az_mode -#define Az_Mode (sdt->az_mode) // azimuth reverce +#define Az_Mode (sdt->az_mode) // azimuth reverse enum{ Rev_Off = 0 // move by nearest way ,Rev_On // move by longest way @@ -425,12 +419,12 @@ enum{ #define mod_vel_D (sdt->simvelf) // telescope & hand correction state /* - * 0x8000 - азимут положительный + * 0x8000 - Az > 0 * 0x4000 - отработка вкл. - * 0x2000 - режим ведения - * 0x1000 - отработка P2 вкл. - * 0x01F0 - ск.корр. 0.2 0.4 1.0 2.0 5.0("/сек) - * 0x000F - напр.корр. +Z -Z +A -A + * 0x2000 - tracking mode + * 0x1000 - P2 auto on + * 0x01F0 - corr. speeds 0.2 0.4 1.0 2.0 5.0("/sec) + * 0x000F - corr. +Z -Z +A -A */ #define code_KOST (sdt->kost) // different time (UTC, stellar, local) @@ -698,69 +692,69 @@ struct BTA_Data { int32_t magic; // magic value int32_t version; // BTA_Data_Ver int32_t size; // sizeof(struct BTA_Data) - int32_t pid; // main process PID - int32_t model; // model modes - int32_t timer; // timer selected - int32_t system; // main system mode - int32_t sys_target; // system pointing target - int32_t tel_focus; // telescope focus type - double pc_coeff[8]; // pointing correction system coefficients - int32_t tel_state; // telescope state - int32_t req_state; // new (required) state - int32_t tel_hard_state; // Power state - int32_t tel_mode; // telescope mode - int32_t az_mode; // azimuth reverce - int32_t p2_state; // P2 motor state - int32_t p2_req_mode; // P2 required state - int32_t focus_state; // focus motor state - int32_t dome_state; // dome motors state - int32_t pcor_mode; // pointing correction mode - int32_t trkok_mode; // tracking mode - double i_alpha, i_delta; // input values - double s_alpha, s_delta; // source - double v_alpha, v_delta; // intrinsic vel. - double i_azim, i_zdist; // input A/Z - double c_alpha, c_delta; // calculated values - double tag_a, tag_z, tag_p; // current values (from sensors) - double pcor_a, pcor_z, refr_z; // calculated corrections - double tcor_a, tcor_z, tref_z; // reverse calculation corr. - double diff_a, diff_z, diff_p; // coords difference - double vbasea,vbasez,vbasep; // base object velocity - double diffva,diffvz,diffvp; // correction by real speed - double speeda,speedz,speedp; // motor speed - double m_time_precip; // last precipitation time - uint8_t reserve[16]; // reserved - double rspeeda, rspeedz, rspeedp; // real motor speed (''/sec) - double simvela, simvelz, simvelp, simvelf, simveld; // model speed - uint32_t kost; // telescope & hand correction state - double m_time, s_time, l_time; // different time (UTC, stellar, local) - uint32_t ppndd_a, ppndd_z, ppndd_p, ppndd_b; // PPNDD sensor (rough) code - uint32_t dup_a, dup_z, dup_p, dup_f, dup_d; // DUP sensor (precise) code (Gray code) - uint32_t low_a, low_z, low_p, low_f, low_d; // binary 14-digit precise code - uint32_t code_a, code_z, code_p, code_b, code_f, code_d; // binary 23-digit rough code - uint32_t adc[8]; // ADC PCL818 (8-channel) codes - double val_a, val_z, val_p, val_b, val_f, val_d; - double val_t1, val_t2, val_t3, val_wnd; // calculated values - double val_alp, val_del; // RA/Decl calculated by A/Z - double vel_a, vel_z, vel_p, vel_f, vel_d; // measured speed + int32_t pid; // main process PID [ServPID] + int32_t model; // model modes [UseModel] + int32_t timer; // timer selected [ClockType] + int32_t system; // main system mode [Sys_Mode] + int32_t sys_target; // system pointing target [Sys_Target] + int32_t tel_focus; // telescope focus type [Tel_Focus] + double pc_coeff[8]; // pointing correction system coefficients [PosCor_Coeff] + int32_t tel_state; // telescope state [Tel_State] + int32_t req_state; // new (required) state [Req_State] + int32_t tel_hard_state; // Power state [Tel_Hardware] + int32_t tel_mode; // telescope mode [Tel_Mode] + int32_t az_mode; // azimuth reverse [Az_Mode] + int32_t p2_state; // P2 motor state [P2_State] + int32_t p2_req_mode; // P2 required state [P2_Mode] + int32_t focus_state; // focus motor state [Foc_State] + int32_t dome_state; // dome motors state [Dome_State] + int32_t pcor_mode; // pointing correction mode [Pos_Corr] + int32_t trkok_mode; // tracking mode [TrkOk_Mode] + double i_alpha, i_delta; // input values [InpAlpha, InpDelta] + double s_alpha, s_delta; // source [SrcAlpha, SrcDelta] + double v_alpha, v_delta; // intrinsic vel. [VelAlpha, VelDelta] + double i_azim, i_zdist; // input A/Z [InpAzim, InpZdist] + double c_alpha, c_delta; // calculated values [CurAlpha, CurDelta] + double tag_a, tag_z, tag_p; // current values (from sensors) [tag_A, tag_Z, tag_P] + double pcor_a, pcor_z, refr_z; // calculated corrections [pos_cor_A, pos_cor_Z, refract_Z] + double tcor_a, tcor_z, tref_z; // reverse calculation corr. [tel_cor_A, tel_cor_Z, tel_ref_Z] + double diff_a, diff_z, diff_p; // coords difference [Diff_A, Diff_Z, Diff_P] + double vbasea,vbasez,vbasep; // base object velocity [vel_objA, vel_objZ, vel_objP] + double diffva,diffvz,diffvp; // correction by real speed [diff_vA, diff_vZ, diff_vP] + double speeda,speedz,speedp; // motor speed [speedA, speedZ, speedP] + double m_time_precip; // last precipitation time [Precip_time] + uint8_t reserve[16]; // reserved [Reserve] + double rspeeda, rspeedz, rspeedp; // real motor speed (''/sec) [req_speedA, req_speedZ, req_speedP] + double simvela, simvelz, simvelp, simvelf, simveld; // model speed [mod_vel_A, mod_vel_Z, mod_vel_P, mod_vel_F, mod_vel_D] + uint32_t kost; // telescope & hand correction state [code_KOST] + double m_time, s_time, l_time; // different time (UTC, stellar, local) [M_time, S_time, L_time] + uint32_t ppndd_a, ppndd_z, ppndd_p, ppndd_b; // PPNDD sensor (rough) code [ppndd_A, ppndd_Z, ppndd_P, ppndd_B (pressure)] + uint32_t dup_a, dup_z, dup_p, dup_f, dup_d; // DUP sensor (precise) code (Gray code) [dup_A, dup_Z, dup_P, dup_F, dup_D] + uint32_t low_a, low_z, low_p, low_f, low_d; // binary 14-digit precise code [low_A, low_Z, low_P, low_F, low_D] + uint32_t code_a, code_z, code_p, code_b, code_f, code_d; // binary 23-digit rough code [code_A, code_Z, code_P, code_B, code_F, code_D] + uint32_t adc[8]; // ADC PCL818 (8-channel) codes [ADC(N): code_T1, code_T2, code_T3, code_Wnd] + double val_a, val_z, val_p, val_b, val_f, val_d; // calculated values [val_A, val_Z, val_P, val_B, val_F, val_D] + double val_t1, val_t2, val_t3, val_wnd; // calculated values [val_T1, val_T2, val_T3, val_Wnd] + double val_alp, val_del; // RA/Decl calculated by A/Z [val_Alp, val_Del] + double vel_a, vel_z, vel_p, vel_f, vel_d; // measured speed [vel_A, vel_Z, vel_P, vel_F, vel_D] // system messages queue struct SysMesg { int32_t seq_num; char type; // message type char text[MesgLen]; // message itself - } sys_msg_buf[MesgNum]; + } sys_msg_buf[MesgNum]; // Sys_Mesg(N) // access levels - uint32_t code_lev[5]; + uint32_t code_lev[5]; // [code_Lev(x): code_Lev1, code_Lev2, code_Lev3, code_Lev4, code_Lev5] // network settings - uint32_t netmask, netaddr, acsmask, acsaddr; - int32_t meteo_stat; // meteo data - double inp_b, inp_t1, inp_t2, inp_t3, inp_wnd; // input meteo values - double temper, press; // values used for refraction calculation - double m_time10, m_time15; // last wind gust time - double dut1; // IERS DUT1 (src: ftp://maia.usno.navy.mil/ser7/ser7.dat), DUT1 = UT1-UTC - double a_time, z_time, p_time; // sensors reading time - double speedain, speedzin, speedpin; // input speeds - double acc_a, acc_z, acc_p, acc_f, acc_d; // acceleration (''/sec^2) + uint32_t netmask, netaddr, acsmask, acsaddr; // NetMask, NetWork, ACSMask, ACSNet] + int32_t meteo_stat; // meteo data [MeteoMode] + double inp_b, inp_t1, inp_t2, inp_t3, inp_wnd; // input meteo values [inp_B, inp_T1, inp_T2, inp_T3, inp_Wnd] + double temper, press; // values used for refraction calculation [Temper, Pressure] + double m_time10, m_time15; // last wind gust time [Wnd10_time, Wnd15_time] + double dut1; // IERS DUT1 (src: ftp://maia.usno.navy.mil/ser7/ser7.dat), DUT1 = UT1-UTC [DUT1] + double a_time, z_time, p_time; // sensors reading time [A_time, Z_time, P_time] + double speedain, speedzin, speedpin; // input speeds [speedAin, speedZin, speedPin] + double acc_a, acc_z, acc_p, acc_f, acc_d; // acceleration (''/sec^2) [acc_A, acc_Z, acc_P, acc_F, acc_D] uint32_t code_sew; // SEW code struct SEWdata { // sew data int32_t status; @@ -781,7 +775,6 @@ struct BTA_Data { double jdate, eetime; // current Julian date, sidereal time correction by "Equation of the Equinoxes" double val_hmd, inp_hmd; // humidity value (%%) & hand input double worm_a, worm_z; // worm position, mkm - /* флаги блокировки управления узлами */ uint32_t lock_flags; // locking flags int32_t sew_dome_speed; // SEW dome divers speed: D_Lplus, D_Hminus etc int32_t sew_dome_num; // SEW dome drive number (for indication) @@ -804,9 +797,9 @@ extern volatile struct BTA_Data *sdt; // Local data structure struct BTA_Local { - uint8_t reserve[120]; // reserved data - double pr_oil_a,pr_oil_z,pr_oil_t; // Oil pressure - double t_oil_1,t_oil_2; // Oil themperature + uint8_t reserve[120]; // reserved data + double pr_oil_a,pr_oil_z,pr_oil_t; // Oil pressure + double t_oil_1,t_oil_2; // Oil themperature }; /** @@ -846,6 +839,4 @@ void set_acckey(uint32_t newkey); // restore packing #pragma pack(pop) -//#pragma GCC diagnostic pop -#endif // __BTA_SHDATA_H__ diff --git a/BTA_dome_modbus/client.c b/BTA_dome_modbus/client.c index 8137e7a..bf8197c 100644 --- a/BTA_dome_modbus/client.c +++ b/BTA_dome_modbus/client.c @@ -31,7 +31,7 @@ #define NEW_HANDLER(name, unused) \ -static const char* cmd_ ## name = STR(name); +static const char* _U_ cmd_ ## name = STR(name); HANDLERS_LIST() #undef NEW_HANDLER @@ -310,8 +310,10 @@ static int process_system(SSL *ssl){ #ifdef EBUG //if(PEP_K_On != Ival.i) DBG("Set PEP_K_On to %d", !Ival.i); #endif + /* if(Ival.i) PEP_K_On = 0; else PEP_K_On = 1; + */ static int modelused = FALSE; if(!G.emulmode){ if(UseModel == FullModel){ // model diff --git a/BTA_dome_modbus/esq770.h b/BTA_dome_modbus/esq770.h index 9948b17..0f125e6 100644 --- a/BTA_dome_modbus/esq770.h +++ b/BTA_dome_modbus/esq770.h @@ -52,6 +52,9 @@ // REG_STATUS_EXT bit #define STATUS_READY_MASK 0x01 -#define FREQ_SCALE 100. +// convert rev/min to frequency register value and back +// 100 * revmin / 60 * 4; 100 - scale, 60 - min2sec, 4 - pole amount +#define REVMIN2FREQ(r) (uint16_t)((100. * (r)) / 15.) +#define FREQ2REVMIN(f) ((((double)(f)) * 15.) / 100.) #define CURRENT_SCALE 10. diff --git a/BTA_dome_modbus/handlers_list.h b/BTA_dome_modbus/handlers_list.h index 857fd0b..b6a0b7e 100644 --- a/BTA_dome_modbus/handlers_list.h +++ b/BTA_dome_modbus/handlers_list.h @@ -30,6 +30,7 @@ NEW_HANDLER(motcurrent, "maximal motor current") \ NEW_HANDLER(motnum, "active motor number for status requests") \ NEW_HANDLER(motspeed, "motor speed") \ NEW_HANDLER(motstatus, "motor status") \ +NEW_HANDLER(nmotors, "amount of working motors") \ NEW_HANDLER(speed, "speed setter") \ NEW_HANDLER(stop, "stop motors") \ diff --git a/BTA_dome_modbus/motors.c b/BTA_dome_modbus/motors.c index aff04e6..a85e07b 100644 --- a/BTA_dome_modbus/motors.c +++ b/BTA_dome_modbus/motors.c @@ -18,7 +18,6 @@ #include #include -#include #include "motors.h" #include "esq770.h" @@ -38,6 +37,8 @@ current=xx - (sg) static modbus_t *modbus_ctx = NULL; static motor_state_t motstates[MOTORS_AMOUNT] = {0}; +// amount of working motors +static int n_working_motors = 0; // set points static double currentSet = DEFAULT_CURRENT, speedSet = 0.; @@ -122,14 +123,15 @@ int motors_set_curntsetpoint(double val){ double motors_get_speedsetpoint(){ return speedSet; } -// set setpoint of speed -int motors_set_speedsetpoint(double val){ +// set setpoint of speed (rev/min) +sl_sock_hresult_e motors_set_speedsetpoint(double val){ double absval = fabs(val); - if(absval > MAX_SPEED) return FALSE; + if(absval > MAX_SPEED) return RESULT_BADVAL; + if(n_working_motors < MIN_WORKING_MOTORS) return RESULT_FAIL; DBG("Change speed setpoint to %g", val); speedSet = val; flags.change_speed = 1; - return TRUE; + return RESULT_OK; } // get number of active motor @@ -160,21 +162,32 @@ int motors_get_actstatus(int *val){ return TRUE; } +// count amount of actual motors +static void count_motors(){ + n_working_motors = 0; + for(int i = 0; i < MOTORS_AMOUNT; ++i){ + if(motstates[i].status != MOT_OFF) ++n_working_motors; + } +} + // main motors processing routine static void motors_process_m(){ static int curN = 0, errctr = 0; static int error_cnt[MOTORS_AMOUNT] = {0}; if(!modbus_ctx || errctr > 100){ - LOGERR("Modbus not inited or other error!"); + LOGERR("Modbus not inited or other error! Error counter = %d", errctr); ERRX("Modbus not inited or other error!"); } if(flags.all){ - if(-1 == modbus_set_slave(modbus_ctx, 0)) goto reg_error; + if(-1 == modbus_set_slave(modbus_ctx, 0)){ + WARN("Can't set broadcast slave"); + goto reg_error; + } if(flags.stop){ DBG("User asks to stop"); speedSet = 0.; - // send command stop - if(-1 == modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP)) goto reg_error; + // send command stop (don't check return code for broadcast messages! + modbus_write_register(modbus_ctx, REG_CMD, CMD_STOP); flags.stop = 0; } if(flags.change_current){ @@ -186,43 +199,68 @@ static void motors_process_m(){ DBG("User asks to change speed to %g", speedSet); // send command "set speed" uint16_t dir = (speedSet > 0.) ? CMD_FORWARD : CMD_REVERSE; - uint16_t freq = (uint16_t)(fabs(speedSet) * FREQ_SCALE); - if(-1 == modbus_write_register(modbus_ctx, REG_CMD, dir)) goto reg_error; - if(-1 == modbus_write_register(modbus_ctx, REG_FREQ_SET, freq)) goto reg_error; + uint16_t freq = REVMIN2FREQ(fabs(speedSet)); + modbus_write_register(modbus_ctx, REG_CMD, dir); + modbus_write_register(modbus_ctx, REG_FREQ_SET, freq); flags.change_speed = 0; } } // set slave N - if(-1 == modbus_set_slave(modbus_ctx, MOTOR_ID(curN))) goto reg_error; + if(-1 == modbus_set_slave(modbus_ctx, MOTOR_ID(curN))){ + WARN("Can't set slave %d", MOTOR_ID(curN)); + goto reg_error; + } // ask for speed/status/current uint16_t regs[2]; - if(-1 == modbus_read_registers(modbus_ctx, REG_STATUS_MAIN, 2, regs)){ + if(-1 == modbus_read_registers(modbus_ctx, REG_STATUS_MAIN, 1, regs) || + -1 == modbus_read_registers(modbus_ctx, REG_STATUS_EXT, 1, ®s[1])){ + // WARN("err in # %d (errno=%d)", curN, errno); if((++error_cnt[curN]) > MAX_ERRORS){ // not answer - set MOT_OFF status - motstates[curN].status = MOT_OFF; - LOGDBG("Motor %d not responce", curN); + if(motstates[curN].status != MOT_OFF){ + motstates[curN].status = MOT_OFF; + LOGDBG("Motor %d not responce", curN); + DBG("Motor %d not responce", curN); + } + error_cnt[curN] = 0; + } + if(++curN >= MOTORS_AMOUNT){ + curN = 0; + count_motors(); } - if(++curN >= MOTORS_AMOUNT) curN = 0; return; } + DBG("motor %d; status: %x, extstatus: %x", curN, regs[0], regs[1]); error_cnt[curN] = 0; switch(regs[0]){ case STATUS_CW: case STATUS_CCW: + DBG("status: RUN"); motstates[curN].status = MOT_RUN; break; case STATUS_STOP: + DBG("status: STOP"); motstates[curN].status = MOT_SLEEP; break; default: + DBG("status: ERROR"); motstates[curN].status = MOT_ERROR; } - if(regs[1] & STATUS_READY_MASK) motstates[curN].status = MOT_ERROR; - if(motstates[curN].status == MOT_ERROR) modbus_write_register(modbus_ctx, REG_CMD, CMD_RESET_FAULT); + if(regs[1] & STATUS_READY_MASK){ + DBG("Motor not ready"); + motstates[curN].status = MOT_ERROR; + } + if(motstates[curN].status == MOT_ERROR){ + if(-1 == modbus_write_register(modbus_ctx, REG_CMD, CMD_RESET_FAULT)) + WARN("Can't reset fault"); + } if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_FREQ, 1, ®s[0])) regs[0] = 0; if(-1 == modbus_read_registers(modbus_ctx, REG_OUTPUT_CURRENT, 1, ®s[1])) regs[1] = 0; - motstates[curN].speed = ((double)regs[0]) / FREQ_SCALE; + motstates[curN].speed = FREQ2REVMIN(regs[0]); motstates[curN].current = ((double)regs[1]) / CURRENT_SCALE; - if(++curN >= MOTORS_AMOUNT) curN = 0; + if(++curN >= MOTORS_AMOUNT){ + curN = 0; + count_motors(); + } errctr = 0; return; reg_error: @@ -279,3 +317,5 @@ void set_emulation_mode(){ motors_close = motors_close_e; motors_process = motors_process_e; } + +int motors_get_working_amount(){ return n_working_motors; } diff --git a/BTA_dome_modbus/motors.h b/BTA_dome_modbus/motors.h index 485945f..67e4c69 100644 --- a/BTA_dome_modbus/motors.h +++ b/BTA_dome_modbus/motors.h @@ -18,6 +18,11 @@ #pragma once +#include + +// minimal amount of working motors to spin dome +#define MIN_WORKING_MOTORS 4 + // max errors per motor to mean it OFF #define MAX_ERRORS 5 @@ -60,7 +65,7 @@ double motors_get_curntsetpoint(); int motors_set_curntsetpoint(double); double motors_get_speedsetpoint(); -int motors_set_speedsetpoint(double); +sl_sock_hresult_e motors_set_speedsetpoint(double); int motors_get_activenum(); int motors_set_activenum(int); @@ -69,6 +74,7 @@ void motors_stop(); int motors_get_actcurrent(double*); int motors_get_actspeed(double*); int motors_get_actstatus(int*); +int motors_get_working_amount(); extern void (*motors_process)(); diff --git a/BTA_dome_modbus/server.c b/BTA_dome_modbus/server.c index 095858c..9a4abab 100644 --- a/BTA_dome_modbus/server.c +++ b/BTA_dome_modbus/server.c @@ -281,8 +281,8 @@ sl_sock_hresult_e speed_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ double D; if(ISSETTER(value)){ if(forbidden) return RESULT_FAIL; - if(!sl_str2d(&D, value) || !motors_set_speedsetpoint(D)) return RESULT_BADVAL; - return RESULT_OK; + if(!sl_str2d(&D, value)) return RESULT_BADVAL; + return motors_set_speedsetpoint(D); } snprintf(value, SL_VAL_LEN-1, "%.3f", motors_get_speedsetpoint()); return RESULT_SILENCE; @@ -293,6 +293,11 @@ sl_sock_hresult_e stop_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ return RESULT_OK; } +sl_sock_hresult_e nmotors_handler(int _U_ index, char _U_ value[SL_VAL_LEN]){ + snprintf(value, SL_VAL_LEN-1, "%d", motors_get_working_amount()); + return RESULT_SILENCE; +} + // binary search handler by name static int search_handler(const char *name){ int low = 0;