diff --git a/libraries/AP_Scripting/examples/6DoF_roll_pitch.lua b/libraries/AP_Scripting/examples/6DoF_roll_pitch.lua new file mode 100644 index 0000000000..c01e82a2d3 --- /dev/null +++ b/libraries/AP_Scripting/examples/6DoF_roll_pitch.lua @@ -0,0 +1,60 @@ +-- A script for controlling the roll and pitch of 6DoF vehicles +-- The script sets the target roll and pitch to the current value when arming or when E-stop is removed +-- this allows the vehicle to be placed upside-down on the ground and to take off as normal +-- its is also possible to setup a switch with option 300 to use either 4 or 6DoF control + +-- make sure the vehicle is capable of 6DoF control +assert(param:get('FRAME_CLASS') == 16, "script requires a 6DoF vehicle") + +local e_stop = rc:find_channel_for_option(31) +local sw = rc:find_channel_for_option(300) +local motors_spinning = false + +function update() -- this is the loop which periodically runs + + -- motor state + local current_motors_spinning = false + + if arming:is_armed() then + -- if armed then motors are spinning + current_motors_spinning = true + end + + if e_stop then + -- if E-stop switch is setup + if e_stop:get_aux_switch_pos() == 2 then + -- E-stop on, motors stopped + current_motors_spinning = false + end + end + + if not motors_spinning and current_motors_spinning then + -- Just armed or removed E-stop + -- set offsets to current attitude + local roll = math.deg(ahrs:get_roll()) + local pitch = math.deg(ahrs:get_pitch()) + attitude_control:set_offset_roll_pitch(roll,pitch) + gcs:send_text(0, string.format("Set Offsets Roll: %0.1f, Pitch: %0.1f",roll,pitch)) + + if sw then + if sw:get_aux_switch_pos() == 2 then + -- switch to 'normal' 4 DoF attitude control + gcs:send_text(0, "4 DoF attitude control") + attitude_control:set_lateral_enable(false) + attitude_control:set_forward_enable(false) + + else + -- 6DoF attutude control + gcs:send_text(0, "6 DoF attitude control") + attitude_control:set_lateral_enable(true) + attitude_control:set_forward_enable(true) + end + end + + end + motors_spinning = current_motors_spinning + + return update, 100 -- 10hz +end + +return update() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index f1ab547d58..d8416b3c0e 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -304,7 +304,7 @@ singleton QuadPlane alias quadplane singleton QuadPlane depends APM_BUILD_TYPE(APM_BUILD_ArduPlane) singleton QuadPlane method in_vtol_mode boolean -include AP_Motors/AP_MotorsMatrix.h +include AP_Motors/AP_MotorsMatrix.h depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_TYPE(APM_BUILD_ArduCopter) singleton AP_MotorsMatrix depends APM_BUILD_TYPE(APM_BUILD_ArduPlane)||APM_BUILD_TYPE(APM_BUILD_ArduCopter) singleton AP_MotorsMatrix alias MotorsMatrix singleton AP_MotorsMatrix method init boolean uint8_t 0 (AP_MOTORS_MAX_NUM_MOTORS-1) @@ -314,3 +314,11 @@ include AP_Frsky_Telem/AP_Frsky_SPort.h singleton AP_Frsky_SPort alias frsky_sport singleton AP_Frsky_SPort method sport_telemetry_push boolean uint8_t 0 UINT8_MAX uint8_t 0 UINT8_MAX uint16_t 0 UINT16_MAX int32_t -INT32_MAX INT32_MAX singleton AP_Frsky_SPort method prep_number uint16_t int32_t INT32_MIN INT32_MAX uint8_t 0 UINT8_MAX uint8_t 0 UINT8_MAX + +include AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h depends APM_BUILD_TYPE(APM_BUILD_ArduCopter) +singleton AC_AttitudeControl_Multi_6DoF depends APM_BUILD_TYPE(APM_BUILD_ArduCopter) +singleton AC_AttitudeControl_Multi_6DoF alias attitude_control +singleton AC_AttitudeControl_Multi_6DoF method set_lateral_enable void boolean +singleton AC_AttitudeControl_Multi_6DoF method set_forward_enable void boolean +singleton AC_AttitudeControl_Multi_6DoF method set_offset_roll_pitch void float -FLT_MAX FLT_MAX float -FLT_MAX FLT_MAX +