Skip to content

Instantly share code, notes, and snippets.

@davidbitton

davidbitton/motor_mix.lua

Created September 29, 2021 20:33
Show Gist options

  • Select an option

    Learn more about clone URLs
  • Save davidbitton/b1351cee82f22b3e9883dba987fb928f to your computer and use it in GitHub Desktop.

Select an option

Learn more about clone URLs
Save davidbitton/b1351cee82f22b3e9883dba987fb928f to your computer and use it in GitHub Desktop.
Download ZIP

Revisions

  1. davidbitton created this gist Sep 29, 2021.
    53 changes: 53 additions & 0 deletions motor_mix.lua
    View file
    Original file line number Diff line number Diff line change
    @@ -0,0 +1,53 @@
    -- duplicate the #defines from AP_Motors
    local AP_MOTORS_MOT_1 = 0
    local AP_MOTORS_MOT_2 = 1
    local AP_MOTORS_MOT_3 = 2
    local AP_MOTORS_MOT_4 = 3

    local function add_motor(motor_num, angle_degrees, yaw_factor, testing_order)

    gcs:send_text(6, string.format("Adding motor:%i to MotorsMatrix", motor_num + 1))

    MotorsMatrix:add_motor_raw(
    motor_num,
    math.cos(math.rad(angle_degrees + 90)),
    math.cos(math.rad(angle_degrees)),
    yaw_factor,
    testing_order
    )

    gcs:send_text(6, string.format("Added motor:%i to MotorsMatrix", motor_num + 1))

    end

    -- This script fails unless the correct FRAME_CLASS is set
    local message = 'Empty message'

    if not(param:set('FRAME_CLASS', 15)) then
    message = 'Unable to set FRAME_CLASS to 15'
    else
    message = 'FRAME_CLASS Succesfully set to 15'
    end

    gcs:send_text(6, message)
    assert((param:get('FRAME_CLASS') == 15), 'Incorrect FRAME_CLASS')

    --[[
    default V (Discovery) definition:
    add_motor(AP_MOTORS_MOT_1, 45, 0.7981f, 1);
    add_motor(AP_MOTORS_MOT_2, -135, 1.0000f, 3);
    add_motor(AP_MOTORS_MOT_3, -45, -0.7981f, 4);
    add_motor(AP_MOTORS_MOT_4, 135, -1.0000f, 2);
    --]]

    gcs:send_text(6, "MotorsMatrix initializing")

    add_motor(AP_MOTORS_MOT_1, 45, -0.7981, 1)
    add_motor(AP_MOTORS_MOT_2, -135, -1.0000, 3)
    add_motor(AP_MOTORS_MOT_3, -45, 0.7981, 4)
    add_motor(AP_MOTORS_MOT_4, 135, 1.0000, 2)

    assert(MotorsMatrix:init(4), "Failed to init MotorsMatrix")

    gcs:send_text(6, "MotorsMatrix initialized")