def clear_comutation_params(link):
parameters = MAVParmDict()
# Set all commutation calibration parameters to 0
parameters.mavset(link.file, "GMB_YAW_SLOPE", 0.0, 3)
parameters.mavset(link.file, "GMB_YAW_ICEPT", 0.0, 3)
parameters.mavset(link.file, "GMB_ROLL_SLOPE", 0.0, 3)
parameters.mavset(link.file, "GMB_ROLL_ICEPT", 0.0, 3)
parameters.mavset(link.file, "GMB_PITCH_SLOPE", 0.0, 3)
parameters.mavset(link.file, "GMB_PITCH_ICEPT", 0.0, 3)
commit_to_flash(link)
def restore_defaults(link):
parameters = MAVParmDict()
parameters.mavset(link.file, "GMB_CUST_GAINS", 0.0); # do not use custom gains for default values
setup_param.commit_to_flash(link)
setup_mavlink.reset_gimbal(link)
return True
def set_param(link, param_name, param_value):
parameters = MAVParmDict()
parameters.mavset(link.file, param_name, param_value, 3)
def restore_params(link, params):
parameters = MAVParmDict()
# Accel params
parameters.mavset(link.file, "GMB_OFF_ACC_X",
params['accel']['offset']['x'], 3)
parameters.mavset(link.file, "GMB_OFF_ACC_Y",
params['accel']['offset']['y'], 3)
parameters.mavset(link.file, "GMB_OFF_ACC_Z",
params['accel']['offset']['z'], 3)
parameters.mavset(link.file, "GMB_GN_ACC_X", params['accel']['gain']['x'],
3)
parameters.mavset(link.file, "GMB_GN_ACC_Y", params['accel']['gain']['y'],
3)
parameters.mavset(link.file, "GMB_GN_ACC_Z", params['accel']['gain']['z'],
3)
parameters.mavset(link.file, "GMB_ALN_ACC_X",
params['accel']['alignment']['x'], 3)
parameters.mavset(link.file, "GMB_ALN_ACC_Y",
params['accel']['alignment']['y'], 3)
parameters.mavset(link.file, "GMB_ALN_ACC_Z",
params['accel']['alignment']['z'], 3)
# Commutation params
parameters.mavset(link.file, "GMB_YAW_SLOPE", params['yaw']['slope'], 3)
parameters.mavset(link.file, "GMB_YAW_ICEPT", params['yaw']['icept'], 3)
parameters.mavset(link.file, "GMB_ROLL_SLOPE", params['roll']['slope'], 3)
parameters.mavset(link.file, "GMB_ROLL_ICEPT", params['roll']['icept'], 3)
parameters.mavset(link.file, "GMB_PITCH_SLOPE", params['pitch']['slope'],
3)
parameters.mavset(link.file, "GMB_PITCH_ICEPT", params['pitch']['icept'],
3)
# Gyro
parameters.mavset(link.file, "GMB_OFF_GYRO_X", params['gyro']['x'], 3)
parameters.mavset(link.file, "GMB_OFF_GYRO_Y", params['gyro']['y'], 3)
parameters.mavset(link.file, "GMB_OFF_GYRO_Z", params['gyro']['z'], 3)
# Joint
parameters.mavset(link.file, "GMB_OFF_JNT_X", params['gyro']['x'], 3)
parameters.mavset(link.file, "GMB_OFF_JNT_Y", params['gyro']['y'], 3)
parameters.mavset(link.file, "GMB_OFF_JNT_Z", params['gyro']['z'], 3)
# Serial Number and Assembly Time
if setup_factory_private:
setup_factory_private.set_serial_number(link,
params['serial_number'],
commit=False)
set_param(
link, "GMB_ASM_TIME",
setup_factory_private.uint32_to_float(params['assembly_time']))
# Save the values
commit_to_flash(link)
# Reset the gimbal
setup_mavlink.reset_gimbal(link)
def pos_hold_disable(link):
parameters = MAVParmDict()
parameters.mavset(link.file, "GMB_POS_HOLD", 0, 3)
def set_accel_params(link, p, level):
parameters = MAVParmDict()
parameters.mavset(link.file, "GMB_OFF_ACC_X", p[0], 3)
parameters.mavset(link.file, "GMB_OFF_ACC_Y", p[1], 3)
parameters.mavset(link.file, "GMB_OFF_ACC_Z", p[2], 3)
parameters.mavset(link.file, "GMB_GN_ACC_X", p[3], 3)
parameters.mavset(link.file, "GMB_GN_ACC_Y", p[4], 3)
parameters.mavset(link.file, "GMB_GN_ACC_Z", p[5], 3)
parameters.mavset(link.file, "GMB_ALN_ACC_X", level[0], 3)
parameters.mavset(link.file, "GMB_ALN_ACC_Y", level[1], 3)
parameters.mavset(link.file, "GMB_ALN_ACC_Z", level[2], 3)
commit_to_flash(link)