I think I got it more or less right:
Tomorrow I start the debugging process in the air…
vibration.lua (3.3 KB)
Basically, what the program should do is to reduce both MOT_SPIN_MAX and ANGLE_MAX slowly (1…2 seconds) to 0.8 and 26 degrees during the time that Vibe_z remains above 60, PROVIDED that the total power is above certain limit (600 W).
if the Vibe_z goes below 30, then original normal values are restored.
In the 30…60 region, the copter would fly with intermediate values of MOT_SPIN and ANGLE_MAX.
And there is a switch button which would unconditionally restore the original values.
function update ()
local power_min=600 – minimum power (W) below which we would not touch anything
local power_max=2000 – max power (W) above which we would not touch anything
local MOT_SPIN_MAX_Minimum=0.8 – we should never go below this.
local MOT_SPIN_MAX_Step=0.005 – step for reducing on each cycle the MOT_SPIN, 20 cycles/sec, so 1 second = 0.1
local ANGLE_MAX_Step=20 – step for reducing MAX ANGLE, value of 20 - 4 deg/sec
local ANGLE_MAX_Minimum=2600 – Minimum angle we could use
local voltage=battery:voltage(0)
local current=battery:current_amps(0)
local vibe_z=ahrs:get_vibration():z()
local power=voltage*current
local panic_switch=rc:get_pwm(11) – panic switch, will restore original values
if not original_angle_max then original_angle_max=param:get(‘ANGLE_MAX’) end – we save the ANGLE_MAX from FC first time we begin executing
if panic_switch>1600 then – restore MOT_SPIN_MAX and ANGLE_MAX
param:set (‘MOT_SPIN_MAX’,0.95)
if not original_angle_max then param:set (‘ANGLE_MAX’,3000) – if somehow we do not have original angle max, we will use a safe 30 deg
else
if (original_angle_max>2000) and (original_angle_max<7000) then – safety check
param:set (‘ANGLE_MAX’,original_angle_max)
else
param:set (‘ANGLE_MAX’,3000)
end
end
gcs:send_text(4, 'VIBE PREVENTION CANCELLED')
return update, 50 -- exit here
end
if (power>=power_min) and (power<=power_max) then – we are within power where we can do things
if (vibe_z>60) then – this is the limit above which we have to do something
if not vibration_event then -- this is first entry to a vibration event
vibration_event=1
gcs:send_text(0, 'VIBRATION EVENT')
param:set ('MOT_SPIN_MAX',0.9) -- immediate and fast action required
MY_MOT_SPIN_MAX=0.9
MY_ANGLE_MAX=original_angle_max
else -- continuous reentry, slowly reduce MOT_SPIN_MAX, and ANGLE_MAX till predefined minimums
MY_MOT_SPIN_MAX=MY_MOT_SPIN_MAX-MOT_SPIN_MAX_Step
if MY_MOT_SPIN_MAX<MOT_SPIN_MAX_Minimum then MY_MOT_SPIN_MAX=MOT_SPIN_MAX_Minimum end
param:set ('MOT_SPIN_MAX',MY_MOT_SPIN_MAX)
MY_ANGLE_MAX=MY_ANGLE_MAX-ANGLE_MAX_Step
if MY_ANGLE_MAX<ANGLE_MAX_Minimum then MY_ANGLE_MAX=ANGLE_MAX_Minimum end
param:set ('ANGLE_MAX',MY_ANGLE_MAX)
gcs:send_text(0, 'Ang:' .. MY_ANGLE_MAX .. ' Max_Spin:'.. MY_MOT_SPIN_MAX)
end
end
end
if (vibe_z<30) and (vibration_event==1) then – this is the end of the vibration event, restore original values
vibration_event=nil
gcs:send_text(0, ‘END OF VIBRATION EVENT’)
param:set (‘MOT_SPIN_MAX’,0.95)
if (original_angle_max>2000) and (original_angle_max<7000) then – safety check
param:set (‘ANGLE_MAX’,original_angle_max)
else
param:set (‘ANGLE_MAX’,3000)
end
end
return update, 50
end
return update()
