From 3c8a652538e7b7d113ba4503ba23cf91bc73b939 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 7 Jan 2026 22:33:51 -0900 Subject: [PATCH] before claude --- boards/ark/fpv/extras/ark_fpv_bootloader.bin | Bin 46268 -> 46324 bytes msg/versioned/ActuatorMotors.msg | 3 +- src/drivers/dshot/DShot.cpp | 161 ++++++++++-------- src/drivers/dshot/DShot.h | 12 +- .../HealthAndArmingChecks/checks/escCheck.cpp | 15 +- 5 files changed, 108 insertions(+), 83 deletions(-) diff --git a/boards/ark/fpv/extras/ark_fpv_bootloader.bin b/boards/ark/fpv/extras/ark_fpv_bootloader.bin index d4f13bd42882370f8dc60c5f3f365d43c0d35ff8..946dc6640f8710ecc5db8b91db48e911d4b8c7ec 100755 GIT binary patch delta 5192 zcma)AeN+_Z)xURU79Iq22`V5ev+S-cuIM5VQENqzhs8w~#Z;{^t{>=9Lm(wYY?6w_ z&y!j;Hy@%g(W+IVM%IZllh`VC+pR~Lbaaa2m* zHYgKmW9*!1cSAYFGXK6uQa%o5xv!_VR#Y8tT-TgYTA!|POQ zG{v3Oaycj$!ilZel*>is5B?LzIK*eDHEtI^Oa%|MV1RlbS{ZvgEZvm3#oN^>D*vML z_;fr#d*UlkbPu%)cmmAy4Wcy#nd>`9;x+jyBD{!jXrxa zVN{;8=bWgR!rSk~?F8Gk4x=n8i%1>09?#LrQEfPnl1I-7ohlQb!$P`~n1eZFN?IPB8Fum= z!Jp@eiiLJ0WsZmrOT2k*F~{suE-E+aouo8W=x&nLa-)9>$3~e)bzzjKoa--{d!@5V z`5|2EtWu)kgU%DDfRSf*B9)F=iU+A_j7h*g`s0{mIpcy7|Gf{C>3xRr(qCmRDpJtK zs{>NkBslMhP_Fdry39UI1(Vb@48STXHaebM7<*z6G55(_P~Iaoxe|%W$97^0%^w%Q zMHD^$^)+<^2JZAhmnU*kt&JzC05GCoc%CX;=RhS@A9deT=NARF|B(a?MfSZ<4f=gNX|yq1j!f2cG26flEJ*lcVm3r;mkTu}bq4`%a5V*uK-$!D#K zEeNA5Hah|%oPsioGA1NsRP^F_>9584kxoIehyT$T;S`j#u*^U2g!Wr8U}2|$*-r!e z!D(PG(UTJra4+qekcL!%cBG5Mx!6NdFsu44jr(@Mue{3H1sqc%U^ z3$if~uM8h?_sZ4aUdQm%F{{7o%x!_xxlb7Q*5I@9-FtCc>g9NT_nxug$G@(>7q_$F zo^j{Z;9fWutdm8r`Tj>W1&UD=J-V*JYgSNf@VtMn;a=R%4NrU++-n@?YRs29+)DkN zWM^2t9J(6ZE601yN~{>a+;cVfB53NqzN^9gpsBn2t_EKP=F`-jeOH4g9c8|&!P5Xr zd1$B{6$106;AzLz;9k6`}tG zSW91I&c=CUo@yPpqW4np80hVCT!Un`zerQpGB~|4PYs#6O~9oaEW*F;}3chi*&u?{MA+?HV+DoAQ7?^Po)12e#rtD$fVj zkPi&=hSC=Q{Jba;htZOPG@+;8UtiE8;@7mXXeGWue<^ZBZw=PUCZ5Xs)#a4oJY>2x zu+te)Ax^4{gtLN#N zMKkdn{bo@CK1F?tDx#*80ZS_bc9AN}HsS>OvaA4)QTpPUv5S4c^aKEGvH!`%SyI%o zH-WwSCa}Z)GfS6C_@X~*g&U*l&H`(G6PSZmJ*MIkTDGzbGyG>(iUQ6h->SCFhBd&F z*8q!J3(URx+hl(rzq=OLj&fiv<-oejfz_@9maq<3;W}U+t^@Yzns05sYybbLH>?5w zoz*um_M@}F=89l(ieM@6Ut8-I@b_e0@1FSLX<*rjVENlwV2(tv#E4)qwn!zi7nS!k zx1wd4J^F_zMx@u)=a120ou-=zu3;Q`qE}ERy%i%;X#G#Ex;NdEQ3T5;dilW9yzh82 z(jl)Mjr6WJ3yl=^nlXV)70=<{>Gg_HhWlrM-KN%xBY2XQ|7am@rZYdwq{tY3dI_WPzUKsbJ zX0Ahf#K$<(T@ekw6~V$khZRYj)P4VQ``Nx9mzYppBY-QH8~nv}YV9 z`ls#b#<)2lG=r?4fFg=z-m6ZhS^NGn?s9Lh`bbcBtEz_r(B5hVR^4LYFHti-cLTp?<3aT~s(c~Al&pbSN|kv9-yMX~-9dEh4hk|-P<^#m;;NcV$6r{F zR*I#W*hIyYGVYbW&z_e#Dk27MRp$lO-@L6KJgXD|ssq`yjsAoWQ`w7C(ieptc`HLZ zgShvw4^OJ2J)*0@17=-<_EiUfUG$e9AVH9P{?MzVrBRndZUxp9%9n!m*+KPpp}BHU zZ6oWEX*icw9I3K@FXUFxqmL^n55J{P>!m)o0v=Gteg_%-7ARLqJz`Ir{}w0@hrrg; zVGKYQc%T~`p!nMwA`Kni%6F*vm#1(sb^p?gXDI1t3Lc{AM^h3SLiGy5g>w;3Jz`MZ z)@pDD)jFy=>YnpN?__6C-7uJYdr|ID-Mt`Bs_ao+y%Sxp=I3{SMc2;loxo*2sk}#> z-D`D%GP2LWElyC3q#aF9Ne&sfr}Z&kpZfBE&Q9))b;Zq_#BF>`d5=1hijHOCT-tOj zB_hHHEYU}=9UHy*aS<%Sc!SH}jA>^kGzp3%BzYuLkNWj*b&&AS$CC^n_kr>`J#*YzB=BL*E#7_k z;z!tBAGa@m*hi6rvf+WezkTqRg8o~fy;=mzRuL@n`BfNw3Ub=4Mn)*vkRbTGYuVPioLsFKCQYr> z?s?t!t5?UEz8K@+$%1(Q<{C|1tgc&nq;Em$r#qN{t!RF4W9?$WBM+xd%xL zH3wJK;eC_Z^af1OnBUN?IpE)#QBza2D?S@HbVF7{eoO~U8rfc@NZ`H?j$oilk)%&L zhjPIyIdCfd^u&0fcYt0yF@9!B7;NR&Wv;2iwR#S=Jmrmgnak5O^}PXy*{Y;O$C$?O zR<5aE5BTTi^JXrXV@x{#AEcgGhVRnClgTNk2J}}gq>da!jt%JesR78C)1kk8*3t7P zQ}E|>>SPrzrsSrXSWZitGH{B2TT_jQP1Jk3Ag5~@uvNLh8gqfgO$XML3#=t~@Vo;@ zrUC1o0Zh#W)|CtF$Sh#*&j7Z`|Ku5mFnro%$NKEdRoUfP*|}LcboQ)ub7KXtLlr~T z`g7z#ef{IWGHZaH`4O<#Cx-AF)xhRd4%MfuEq{8-Q%`N0Q!wAU7PNoH)d8Ei5m@?9 zfZcl)9cD)8@g_0LwQ6qfFn(sDv#;$VIAcwr9S- z6_}p%Hnlw{tu<4uHK6_T@-Kj`+70aWpAX?Db^~jmj^^dU{eRMA-#R9r83Zhp=^I5p zU)k7x0W6F$07=Nx>qgz}VqvKyf`v@yPEnrEkQG-vqfTH4_kj!aSX%0-z zzh-wdgQ$E;cY>E<9xa$|#(h*VJr#j=O)oYaI1a3YKAUdFM`?6=2X3VHkgeEGCSwJD zObtd;=y5HJgL26i*@Nv_TvXn6oic_Z)>2RC9-K?LVcqDY(Xh3_*L>1-nOht~gQD^! zRfi|xOSCUM6|d5z@CbZ}u7;a2n|$H(Fo}|9R0k~c$vqDjipml?JR_Oi9p7~~wC%zm zWzU>7GrMr4OH^k1cHa$c1KVePMp;xisWUd>o3wW31^fv`&sq`@KaQ`7Rl;ib8!vPC75TBA}!v~h`d<`r>FFXN)pPrr*w#INa2lyy$ucninFG7lKS08x2!yd>|F z!CK|t;IqM6B@jLxY&rwX*iEuLi^^tK;y!AhZ4!pP^x^Ci8KEADzvl#H(U>7l`ax^%-lR0W%wk#+mKQOWlkZ61`v0moJNk;!a7>g-5J_6q-Yy1k_NcHg<)4R3nJ;p-! z4P&;Tbo5f$QxRc>Uv~-0F>UAF&^B97q%0_((qH1!gc6PRhtE$Glvmu)doc(smxI8P z)h%%tDlyo&psd!)dZeL;C+jO|lWYyl(QG|(y8g*a=&<|<{+5R11=vO=(^A|{>r6=@ ztF`aD^eH3rEI0JF(P2|GR#CUfl$@bqI?D)5`L+*agFZ8Mw4DK#)otKevY>oCE^)h4 z?`A={O_&g`&-R33jHjA}Obn-!31(r;M}J6Ih=JrwSQT}{2Xa-_)XTEmkv!UQb5JVO zzpd6BA9c`&N5Jf7LfKFGnGei(#Lpzie#&k-W!{V>6mB_*5!7SJz%aUFnHhd%yt=C1 zalFdq_^Rp`gJrUy+(!}CM0}f;TFXrXy5Lm9C z0VbXWHvcTJ8hUz81UhKXoJ2hBa?ZJcc$#W#nE|KI0%P5NN(Y^@WyTgvNJ{>Mq~uNn z7e_iGdxDCi9np^Z9+3T&RKlcqG?6JOVrIxV9+tSBYwE0tV4dUiQ=(`^Qh}g)=;uk5 z_&nW7T42uefl@Z^zQM9l`AP3F5iED!J@@DKbbVl^l~^5q$}ecy+{O3;ZJ)at!(BJ$ zh9f#$zT`nfD}6I>rP(`X?2);o6p#IVEb6-RpOQGt+1>e>=LCTA7g{sl6w)=8#^-XW zXv)?@d*|ok9_pJP9=LbRztis4HB5A8{#MMPjSGr!2{{++62`rhWuJpLX|25+-y^4e zW>eEB$VPX#GCkzZC;guN_UQ?;*MB>h)eQ-GPwn(8teDG4$P^kd&c@br|lKae$QC| zrP4c9PWOWOvgfS5-*apd)B9t&w|}yPN$i9@%T32p?a|x4{gcv9d#9vpynmN=(5=*^ z_#h=Ow8pL)z3e#w{hs~MoB$?f_o?aLe$Q!$bb(S#H4DwAbE7(%osh(t{j%qLnNOYJ zMRNl8FPaMJ3^^B?=a+lH9IzsWuk=XKi=2FekLMREc44@Gc&3 z%P)MV2aYB1V~uMX@jip%Z(w7F;TNJ-IecoUPv$SIGN!*;FvBxF$fqvz8s~r0=f0t| zLhpBE$ap1HSG9NNH3pf&mP?Aih+|^H0mnKgHLh@oxB5k}40IiVe2+O>7;dg#?s%OB zGNPM=I(-=J?u!5xlU*3u3V}CZw(LNX4%?A`7-5SnAH_HO9=56n}$Q;x}f< z``U4W*Ilfd#Bv*B5=6yctjNXXS1PCYxs5RN;87(>QLDc#^Yt zlekWF)!uhlz_YG^Mb(HURC~V(7rPGJpNx3Y>TOnWN;D$fCvL&5UEIbb~$Tf7mUrCr5t458eTRe_lr*n$RZ zd|dsMPL){kxa*^m0Sx}rgv^CbqnmY)sod55;8Mgi*Kp}D;w`#Wz7pS~We*hueyM@Y zJ79E=tM3s#v>4x|j~~j#W(u#^6cAAXETjV18QN8`6&I4PA{V#OvWFK3r#OM>2>@7% zYtO@JQoy#ifIa&butwJhm8&H@<}$4*#elL7U~O*!WAxl(DsG^s*H+*n*9U7w0n_RC z>n=2XzaE&l0oc$6V43UxnZh5RsyiPCR$C41Xf?2F)xg#~0Zgj~Htz{w7oGt2yMOWf zvBv*b-@E}_;)ZJ&+}QyvT?9*(2$lz2-#=a|;H$J`V`m^~6K;{yHX z##zF+KyPo%nLRFmMYrk8K+Dfw~>Z2=t{R z4S{BEGNYLmZQ35KjDpPEU@pBk@QhF-6bj1iccAyf4q%t4Z_^uip6Z`0#~0~?C%57} zTC({|{4>RGc?7#?>y`rCLcLqIqMb5pVlkIi)+FN^YN;`qyjmgmszu(v?lr$u$iup# zyN=|<_0~H?1;yB0ulg;$Uz3KPkXn-){>i8mH+$e#qo`>3qN|L1)swX1sasJCwATku z_)RS?FeEdHPv_a(tL9UA?J_~o=;hi345MGyekpv{K}YK1^y6<`Jg#+JtuqLrkNS=` zCOg3_4aLCmIv zxj}Tftj}wRyU4ojeM7sqeXzsz{kEeLZl)Jo3b25#v>XbE=>(SE2`q@7{%LY>kw^b2 zJUedaw7Y|-{imB}wsZo!-3hF*6WI1nU{F|B@RBQO$4SHl`grFKOreThi|`TJyK5fa zrVG0&@G^z$Zo>bjeY?&047qk&(MeZ#C*g7uTa*0{du@DFO{I+1e00zYttJejqph(K zDPv$We4%gUl+uvMk z{$ox&t*WnU$YrjoT#I$)I+Ck)I5t=LJ*TSc>HU|BjIT_{+|M+sMtBRkN1dio%%K$Y zQPH9Js6{X9Cmo6BN6L-Nc{T_7vfP+Hs{Y>9a;O0D*Yx|JSKwZXeWf0+xDLFsUBDR> zawI<>;0Um*hxOHI&5>wB(Gg&0T-%S3ApEJ*6?Sx%G_%iJs=$VP`Ld@u-J|}&n*e@H)9W_9@kIN<;UYA_IaBXgiBq1h58#k z>VY0Zp-0_Gt;b83JwH0X(4%gh%%2@ac|=`33UW%#i2BIr+~U`Ba{9ocTgyi0aG9r6 zkEq3?)#SfM>{HWOO@+pS!v3yTvt~By^qpIC!(z0-#jXU#dsaA>-PaH98rgU z1HF1163`?SvhES}1_hpsN*wryU6|xu`JpY@^m|1R5C2?`rz%M@WYOloPf`97(!ce3|9?>Esg^@ zZM~7au3+*%fZi$*EYFHykGmTALa?_`q;qtr7zI6=i%d zySN0hx;6EZX5eMc5moa^angW|YwDYts=hg<-_o43n%6 zG#gjdgFoVR4mU3~c1wKPr!d?M*{G?ciJO`oU(beHd4kbMC*MqK3bDa(^Ai#;{g(5` zKG!b=Pih7}qPF|MRx~NxrulQQ74+$$EY~FS{XEE|sWsXyN9nz~^+BeuX4`nQAl|#a zK~vXj&6Bv!eN@x)O`62*7ep>41Wj7YH8Y>`o75|qB$ZFb9ybiAX zqW#ACR!&eKq|E58RU|0A2X>FUR*|H?4o>B14(UEjqc=~*38>L;PsJ@x^MS2;K<1hn zrZsc0Wh=jGmbsj%sh>{R%~mBoFvv8U_i#=9p9xo94)5fGImo2*zoq!otMDUw?sW9h zj)_u5OM`?NlgQ}_9q*WcdCU6r)#Q1vjk|An_OT(Eb?A{8ZV&#Lzn`w0uEk2qZC{Mf zQB(Uo%ygY=uNUzRGM&rKxS9#9C<|Ci7O>GQUTRt)7Dk5e@%7g*~-`|jLJz>4+)YyH_2zHcwE z%{1D%+8}+eH6fLE&7~dAMS|xnz3Y4`<~so_j4=R7$kyvdUGrg~@}2+|D(^Z)#&ZTc zrU5gh1FJ~`mXQXmI&Df@fBr9w%Db$tRjvp$9Me}AYB(Q{7wORX_+Vcru%!S{CIL*N IOXt)67elAY-v9sr diff --git a/msg/versioned/ActuatorMotors.msg b/msg/versioned/ActuatorMotors.msg index d165c5c1cb..1685c65f9e 100644 --- a/msg/versioned/ActuatorMotors.msg +++ b/msg/versioned/ActuatorMotors.msg @@ -10,7 +10,8 @@ uint64 timestamp_sample # [us] Sampling timestamp of the data this control resp uint16 reversible_flags # [-] Bitset indicating which motors are configured to be reversible -uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 # +uint8 ACTUATOR_FUNCTION_MOTOR1 = 101 +uint8 ACTUATOR_FUNCTION_MOTOR12 = 112 uint8 NUM_CONTROLS = 12 # float32[12] control # [@range -1, 1] Normalized thrust. where 1 means maximum positive thrust, -1 maximum negative (if not supported by the output, <0 maps to NaN). NaN maps to disarmed (stop the motors) diff --git a/src/drivers/dshot/DShot.cpp b/src/drivers/dshot/DShot.cpp index 5f70c37458..ca3e574d24 100644 --- a/src/drivers/dshot/DShot.cpp +++ b/src/drivers/dshot/DShot.cpp @@ -122,7 +122,7 @@ void DShot::Run() bool DShot::updateOutputs(uint16_t *outputs, unsigned num_outputs, unsigned num_control_groups_updated) { - if (!count_set_bits(_output_mask)) { + if (!_hardware_initialized || !count_set_bits(_output_mask)) { return false; } @@ -204,12 +204,16 @@ void DShot::select_next_command() // EDT Request first int next_motor_index = 0; - for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { - bool is_bdshot = _bdshot_output_mask & (1 << i); + for (int output_channel = 0; output_channel < DSHOT_MAXIMUM_CHANNELS; output_channel++) { + bool is_bdshot = _bdshot_output_mask & (1 << output_channel); - if (is_bdshot && (needs_edt_request_mask & (1 << i))) { - next_motor_index = i; - break; + if (is_bdshot && _mixing_output.isMotor(output_channel)) { + int motor_index = (int)_mixing_output.outputFunction(output_channel) - (int)OutputFunction::Motor1; + + if (needs_edt_request_mask & (1 << motor_index)) { + next_motor_index = motor_index; + break; + } } } @@ -500,7 +504,7 @@ bool DShot::process_serial_telemetry() case TelemetryStatus::ParseError: // Set ESC data to zeroes - PX4_WARN("Telem parse error"); + PX4_WARN("Telem parse error, index %u", _telemetry_motor_index); _serial_telem_errors[_telemetry_motor_index]++; _serial_telem_online_mask &= ~(1 << _telemetry_motor_index); _serial_telem_online_timestamps[_telemetry_motor_index] = 0; @@ -625,7 +629,7 @@ bool DShot::process_bdshot_telemetry() } else { _bdshot_telem_online_mask &= ~(1 << motor_index); - _bdshot_edt_requested_mask &= ~(1 << motor_index); // re-triggers EDT request when it comes back online + // _bdshot_edt_requested_mask &= ~(1 << motor_index); // re-triggers EDT request when it comes back online perf_count(_bdshot_error_perf); } @@ -867,116 +871,121 @@ void DShot::mixerChanged() _esc_status.esc_connectiontype = esc_status_s::ESC_CONNECTION_TYPE_DSHOT; - int motor_count = 0; + // Build output mask from actual motor assignments + uint32_t new_output_mask = 0; for (int i = 0; i < DSHOT_MAXIMUM_CHANNELS; i++) { - if (_mixing_output.isMotor(i)) { _esc_status.esc[i].actuator_function = (uint8_t)_mixing_output.outputFunction(i); - motor_count++; - - if (!(_output_mask & (1 << i))) { - PX4_INFO("Enabling channel %d", i); - } - - _output_mask |= (1 << i); - - } else { - if ((_output_mask & (1 << i))) { - PX4_INFO("Disabling channel %d", i); - } - - _output_mask &= ~(1 << i); + new_output_mask |= (1 << i); } } - // TODO: we should only re-initialize if we gain/lose outputs - // initialize_dshot(); + // Check if we need to (re)initialize hardware + // bool needs_init = !_hardware_initialized || (new_output_mask != _output_mask); + + if (!_hardware_initialized) { + PX4_INFO("Output mask changed: 0x%lx -> 0x%lx", _output_mask, new_output_mask); + _output_mask = new_output_mask; + uint32_t new_bdshot_output_mask = _bdshot_timer_channels & _output_mask; + PX4_INFO("BDShot Output mask changed: 0x%lx -> 0x%lx", _bdshot_output_mask, new_bdshot_output_mask); + _bdshot_output_mask = new_bdshot_output_mask; + + PX4_INFO("up_dshot_init"); + up_dshot_init(_output_mask, _bdshot_output_mask, _dshot_frequency, _bdshot_edt_enabled); + up_dshot_arm(true); + _hardware_initialized = true; + } } bool DShot::initialize_dshot() { - PX4_INFO("initialize_dshot"); - unsigned int dshot_frequency = 0; - uint32_t dshot_frequency_param = 0; + PX4_INFO("initialize_dshot: motor_mask=0x%lx", _output_mask); - _output_mask = 0; - _bdshot_output_mask = 0; + uint32_t dshot_timer_channels = 0; // Channels on DShot-enabled timers + // Iterate through timers to determine DShot frequency and BDShot channels + // _output_mask already contains only motor channels (set by mixerChanged) for (uint8_t timer_index = 0; timer_index < MAX_IO_TIMERS; timer_index++) { // Get mask of actuator channels associated with this timer group - uint32_t channels = io_timer_get_group(timer_index); + uint32_t timer_channels = io_timer_get_group(timer_index); - if (channels == 0) { + if (timer_channels == 0) { continue; } - char param_name[17]; + char param_name[17] = {}; snprintf(param_name, sizeof(param_name), "%s_TIM%u", _mixing_output.paramPrefix(), timer_index); int32_t tim_config = 0; param_t handle = param_find(param_name); param_get(handle, &tim_config); - unsigned int dshot_frequency_request = 0; + // Check if this timer is configured for DShot if (tim_config == -5 || tim_config == -8) { - dshot_frequency_request = DSHOT150; - _output_mask |= channels; + _dshot_frequency = DSHOT150; + dshot_timer_channels |= timer_channels; } else if (tim_config == -4 || tim_config == -7) { - dshot_frequency_request = DSHOT300; - _output_mask |= channels; + _dshot_frequency = DSHOT300; + dshot_timer_channels |= timer_channels; } else if (tim_config == -3 || tim_config == -6) { - dshot_frequency_request = DSHOT600; - _output_mask |= channels; + _dshot_frequency = DSHOT600; + dshot_timer_channels |= timer_channels; } + // Bidirectional DShot (tim_config < -5 means -6, -7, or -8) if (tim_config < -5) { - _bdshot_output_mask |= io_timer_get_group(timer_index); + // Add timer channels that are also motors to BDShot mask + // _bdshot_output_mask |= (timer_channels & _output_mask); + // _bdshot_output_mask |= timer_channels; + _bdshot_timer_channels |= timer_channels; } - if (dshot_frequency_request != 0) { - if (dshot_frequency != 0 && dshot_frequency != dshot_frequency_request) { - PX4_WARN("Only supporting a single frequency (%u), adjusting param %s", dshot_frequency, param_name); - param_set_no_notification(handle, &dshot_frequency_param); - - } else { - dshot_frequency = dshot_frequency_request; - dshot_frequency_param = tim_config; - } - } + PX4_INFO("timer_index %u, channels_mask %lu", timer_index, dshot_timer_channels); } - _bdshot_output_mask &= _output_mask; - int ret = up_dshot_init(_output_mask, _bdshot_output_mask, dshot_frequency, _bdshot_edt_enabled); + PX4_INFO("_bdshot_output_mask %lu", _bdshot_output_mask); - if (ret < 0) { - PX4_ERR("up_dshot_init failed (%i)", ret); - return false; - } - - if ((uint32_t)ret != _output_mask) { - PX4_INFO("Failed to configure some channels"); - PX4_INFO("requested: 0x%lx", _output_mask); - PX4_INFO("configured: 0x%lx", (uint32_t)ret); - _output_mask = ret; - } - - // Set our mixer to explicitly disable channels we do not control - for (uint8_t i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { - if (((1 << i) & _output_mask) == 0) { - _mixing_output.disableFunction(i); - } - } - - if (_output_mask == 0) { + if (dshot_timer_channels == 0) { PX4_WARN("No channels configured"); return false; } - up_dshot_arm(true); + + // _output_mask &= dshot_timer_channels; + + // PX4_INFO("dshot_timer_channels=0x%lx, _output_mask=0x%lx, _bdshot_output_mask=0x%lx", + // dshot_timer_channels, _output_mask, _bdshot_output_mask); + + + // if (ret < 0) { + // PX4_ERR("up_dshot_init failed (%i)", ret); + // return false; + // } + + // if ((uint32_t)ret != _output_mask) { + // PX4_INFO("Failed to configure some channels"); + // PX4_INFO("requested: 0x%lx", _output_mask); + // PX4_INFO("configured: 0x%lx", (uint32_t)ret); + // _output_mask = ret; + // } + + // Set our mixer to explicitly disable channels we do not control + // for (uint8_t i = 0; i < DIRECT_PWM_OUTPUT_CHANNELS; ++i) { + // if (((1 << i) & _output_mask) == 0) { + // _mixing_output.disableFunction(i); + // } + // } + + // if (_output_mask == 0) { + // PX4_WARN("No channels configured"); + // return false; + // } + + // up_dshot_arm(true); return true; } @@ -1180,6 +1189,8 @@ int DShot::task_spawn(int argc, char *argv[]) return PX4_OK; } + PX4_INFO("Exiting"); + } else { PX4_ERR("alloc failed"); } diff --git a/src/drivers/dshot/DShot.h b/src/drivers/dshot/DShot.h index 0be7dde746..a93abb6611 100644 --- a/src/drivers/dshot/DShot.h +++ b/src/drivers/dshot/DShot.h @@ -52,9 +52,9 @@ using namespace time_literals; static constexpr hrt_abstime ESC_INIT_TELEM_DELAY = 5_s; /// Dshot PWM frequency (Hz) -static constexpr unsigned int DSHOT150 = 150000u; -static constexpr unsigned int DSHOT300 = 300000u; -static constexpr unsigned int DSHOT600 = 600000u; +static constexpr uint32_t DSHOT150 = 150000u; +static constexpr uint32_t DSHOT300 = 300000u; +static constexpr uint32_t DSHOT600 = 600000u; static constexpr uint16_t DSHOT_DISARM_VALUE = 0; static constexpr uint16_t DSHOT_MIN_THROTTLE = 1; @@ -159,9 +159,13 @@ private: // Parameters we must load only at init bool _serial_telemetry_enabled = false; - bool _bdshot_edt_enabled = false; + // Hardware initialization state + bool _hardware_initialized = false; + uint32_t _dshot_frequency = 0; + uint32_t _bdshot_timer_channels = 0; + // Perf counters perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; perf_counter_t _bdshot_success_perf{perf_alloc(PC_COUNT, MODULE_NAME": bdshot success")}; diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 9b35c80992..5fa9b4388e 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -114,9 +114,18 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e // Check if one or more the ESCs are offline if (online_bitmask != esc_status.esc_online_flags) { - for (int index = 0; index < esc_status.esc_count; index++) { - if ((esc_status.esc_online_flags & (1 << index)) == 0) { - uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1; + for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { + + uint8_t actuator_function = esc_status.esc[i].actuator_function; + + bool is_motor = math::isInRange(actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1, + actuator_motors_s::ACTUATOR_FUNCTION_MOTOR12); + bool is_online = esc_status.esc_online_flags & (1 << i); + + if (is_motor && !is_online) { + + uint8_t motor_index = actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1; + /* EVENT * @description *