'ToDo: 'get more time to fly ' ' _____ _ _ _ _ _ '/ ___| | | (_) | | | | '\ `--.| |__ _ __ ___ __| |_ __ _ _ _ ___| |_| |_ ___ ' `--. \ '_ \| '__/ _ \/ _` | |/ _` | | | |/ _ \ __| __|/ _ \ '/\__/ / | | | | | __/ (_| | | (_| | |_| | __/ |_| |_| __/ '\____/|_| |_|_| \___|\__,_|_|\__, |\__,_|\___|\__|\__|\___| ' | | ' |_| ' ' By William Thielicke ( w . t h @ g m x . d e ) ' ____________pin Out________________ ' | | ' | Arduino Pin = Mega328p Pin | ' | | ' | Servo - > D6 = Portd.6 | ' | Receiver - > D3 = Portd.3 = Int1 | ' | Led1 - > D7 = Portd.7 | ' | Led2 - > D8 = Portb.0 | ' | Led3 - > D9 = Portb.1 | ' | Sda - > D4 = Portd.4 | ' | Scl - > D5 = Portd.5 | ' | Arduino Led_grn = Portb.5 | ' | Voltage meas = ADC(0) | ' | Gyro Yaw = ADC(1) | ' | Gyro Nick = ADC(2) | ' | Gyro Roll = ADC(3) | ' | X Acc = ADC(4) | ' | Y Acc = ADC(5) | ' |___________________________________| ' Version 1.0 - Shrediquette DLX ' Bremen , Germany , 01.11.2009 ' http://Shrediquette.blogspot.com ' Published under the CreativeCommons Attribution-Noncommercial-Share Alike 3.0 Germany ' http://creativecommons.org/licenses/by-nc-sa/3.0/de/deed.en '-- 'Preface: 'A tricopter is a flying thing that uses three motors + propellers (arranged in a horizontal triangle) to stay airborne. 'The speed of each of these motors can be controlled independently. Additionally, the motor at the rear can be rotated left and right via a servo. 'One propeller turns clockwise, two propellers turn counter-clockwise. 'Gyroscopes (measuring angular velocity) and accelerometers (measuring accelerations) are used to stabilize the inherently unstable 'system. The pilot can control the tricopter with a remote control. Height, nick, roll and yaw are controlled via 2 sticks on the remote control. 'A 3-position switch turns on the motors and selects control mode. Hence a 5-channel transmitter & receiver is needed. 'This specific tricopter uses two different control modes: Firstly, "hover mode" where the pilot controls the angle of the tricopter. Moving 'the sticks 10 degrees tilts the tricopter ~10 degrees. The copter will begin to accelerate in the desired direction. If you release the sticks, 'the copter will automatically level. But it will keep the velocity it had before (if we disregard aerodynamic drag). 'Secondly, "acrobatic mode", where the pilot controls the angular velocity of the copter. Moving the sticks "a bit" will result in slow rotation 'of the tricopter. Moving the sticks further will result in faster rotation. The copter will always keep the angle it had before if you release 'the sticks. Yaw control is always "heading-hold" = "velocity control". 'The source code can be roughly divided in eleven parts: '1 (startup) : Initialization of hardware & variables & subs & startup procedure '2 (gyro) : Measurement of angular velocities via gyroscopes, PID control loop '3 (acc) : Measurement of accelerations (mainly gravitation) '4 (mixer) : Combination of sensor and stick inputs '5 (send_mots): Sending out motor signals via I2C '6 (led) : Flashing of some leds '7 (voltage) : Measuring the voltage of the battery '8 (failsave) : Monitoring receiver (RX) signal '9 (receiver) : Reading RX's sum-signal '10 (servo) : Creation of servo PWM signal '11 (GUI) : Listen to serial commands from the GUI 'Please Note : If you never flew a radio controlled airplane or heli before, practice on a RC-simulator first! Even if the copter is auto-stable, 'there are always situations where you quickly have to react in the right way. You can only learn that by practicing (or by paying a lot of 'money for spare parts...) 'The obligatory end-note: This is not a toy, although it is a lot of fun to play with it... '===CHIP SETTINGS=== $regfile = "m328pdef.dat" '"m168def.dat" $framesize = 32 $swstack = 32 $hwstack = 32 $crystal = 16000000 $baud = 38400 '===READ RX SETTINGS=== Config Timer0 = Timer , Prescale = 256 On Timer0 Detectrxpause 'timer overflow = pause in receiver's signal Config Int1 = Rising On Int1 Getreceiver 'rising edge -> measure time '===SERVO SETTINGS=== Config Timer1 = Timer , Prescale = 8 Timer1 = 62535 On Timer1 Servoirq 'creates the servo PWM '===SERIAL IN SETTINGS=== Config Serialout = Buffered , Size = 254 Config Serialin = Buffered , Size = 254 '===ADC SETTINGS=== Config Adc = Single , Prescaler = Auto , Reference = Avcc 'avcc reference @ arduino =5V '===PORT SETTINGS=== Config Scl = Portd.5 'for ESCs Config Sda = Portd.4 'for ESCs Config Pind.6 = Output 'servo Portd.6 = 0 'servo Config Pind.7 = Output 'led 1 Config Pinb.0 = Output 'led 2 Config Pinb.1 = Output 'led 3 Config Pinb.5 = Output 'Arduino LED_grn '===DECLARATIONS=== '--Global-- Dim I As Word 'counter var Dim Lf As Single 'stick sensitivity Dim Lfdynamic_roll As Single 'dynamic stick sensitvity (used for flying loopings) Dim Lfdynamic_nick As Single 'dynamic stick sensitvity (used for flying loopings) Dim Motors_on As Bit 'self explanatory, right...?! Dim State As Byte 'Contains selected control loop state: 0 = Motors off, 1 = Acrobatic mode, 2 = Hover mode Dim Old_state As Byte 'Contains the state of the last cycle Dim Lookup_pos_nick As Integer Dim Lookup_pos_roll As Integer '--Servo control-- Dim Servotail As Word 'servo position, neutral = 62535, full left = 61535, full right = 63535 Dim Shigh As Bit '1 if servo pin is high, 0 if servo pin is low '--Read Receiver (Rx)-- Dim Empf(5) As Word 'data from getreceiver interrupt Dim Sempf(5) As Integer 'rescaled data Dim Aempfh(5) As Integer 'higher limit for rx noise filter Dim Aempfl(5) As Integer 'lower limit for rx noise filter Dim Channel As Byte 'current channel to be read out Dim Rc_on_counter As Word 'counter that counts the amount of correct rx signals Dim Newvalsreceived As Bit Dim Nickstickvel As Integer Dim Rollstickvel As Integer Dim Sempf_old_nick As Integer Dim Sempf_old_roll As Integer '--I2C-- Const M_h = &H52 'i2c address: motor at the back Const M_l = &H54 'i2c address: motor at the left Const M_r = &H56 'i2c address: motor at the right Dim Ms_h_i As Integer 'contains motor nominal value as integer Dim Ms_r_i As Integer 'contains motor nominal value as integer Dim Ms_l_i As Integer 'contains motor nominal value as integer Dim Ms_h As Word 'converts motor nominal value Dim Ms_r As Word 'converts motor nominal value Dim Ms_l As Word 'converts motor nominal value '--Gyros-- Dim Roll_init As Word 'gyro offset Dim Nick_init As Word 'gyro offset Dim Yaw_init As Word 'gyro offset Dim Meas_roll As Integer 'angular velocity reading from gyros Dim Setpoint_roll As Single 'Stick position Dim Error_roll As Single 'Stick position - current position Dim Error_roll_sum As Single 'Integral of the above Dim Meas_angle_roll As Single 'the angle of the copter including acc signal (only in Hover mode) Dim Meas_nick As Integer 'angular velocity reading from gyros Dim Setpoint_nick As Single 'Stick position Dim Error_nick As Single 'Stick position - current position Dim Error_nick_sum As Single 'Integral of the above Dim Meas_angle_nick As Single 'the angle of the copter including acc signal (only in Hover mode) Dim P_set_roll As Single 'P output including scaling (gain) Dim I_set_roll As Single 'I output including scaling (gain) Dim D_set_roll As Single 'D output including scaling (gain) Dim P_set_roll_int As Integer 'converts to integer Dim I_set_roll_int As Integer 'converts to integer Dim D_set_roll_int As Integer 'converts to integer Dim P_set_nick As Single 'P output including scaling (gain) Dim I_set_nick As Single 'I output including scaling (gain) Dim D_set_nick As Single 'D output including scaling (gain) Dim P_set_nick_int As Integer 'converts to integer Dim I_set_nick_int As Integer 'converts to integer Dim D_set_nick_int As Integer 'converts to integer Dim Yaw_gyro As Integer 'angular velocity reading from gyroscope Dim Yaw_gyro_i As Integer 'yaw gyro var Dim Yaw_gyro_scale As Single 'scaled-down values Dim Yaw_gyro_scale_int As Integer 'converts to integer Dim Yaw_gyro_i_scale As Single 'scaled-down values Dim Yaw_gyro_i_scale_int As Integer 'converts to integer Dim P_sens As Single 'roll/nick P control loop sensitivity Dim I_sens As Single 'roll/nick I control loop sensitivity Dim D_sens As Single 'roll/nick D control loop sensitivity Dim Yaw_p_sens As Single 'yaw P sensitivity Dim Yaw_i_sens As Single 'yaw I sensitivity Dim Gyro_i_enable As Bit '0 or 1: don't perform gyro integration when motors off Dim Error_nick_d(3) As Single Dim Error_roll_d(3) As Single Dim Error_nick_old(3) As Single Dim Error_roll_old(3) As Single Dim Looper As Byte Dim D_sens_acro As Single Dim Dd_sens As Single Dim Dd_set_nick As Single Dim Dd_set_roll As Single Dim Dd_set_nick_int As Integer Dim Dd_set_roll_int As Integer '--Acc-- Dim Xacc As Integer 'accelerometer nick Dim Yacc As Integer 'accelerometer roll Dim Accy_temp As Single 'used for complementary filtering Dim Accx_temp As Single 'used for complementary filtering '--Failsave-- Dim Failure As Byte 'increases when there are errors in receiver reading Dim Meanrx(5) As Word 'used for receiver noise filter '--Mixer-- Dim Vorwahl As Word 'throttle idle up Dim Yaw_diff As Integer 'yaw gyro var Dim Tailgas As Integer 'increases throttle of rear motor if tilted '--Voltage Check-- Dim Spann As Word 'reads voltage from adc Dim Spannmittel As Single 'used for filtering Dim Spann_alt As Word 'used for filtering (alt is german and means "old") Dim Spann_alt2 As Word 'used for filtering Dim Spann_alt3 As Word 'used for filtering Spann_alt = 1023 'prepare the filter Spann_alt2 = 1023 'prepare the filter Spann_alt3 = 1023 'prepare the filter Dim Lowvoltage As Bit 'will be 1 if voltage is low '--LEDs-- Dim Ledcount As Word 'used for bling bling Dim Blinker As Word 'used for low voltage bling bling '--GUI Connection-- Dim A As String * 8 Dim Read_empfangen As Byte Dim Reset_empfangen As Byte Dim Sensors_empfangen As Byte Dim Input_empfangen As Byte Dim Var(33) As Byte Dim Sensor(13) As Byte Dim Motorsenable As Byte Dim Roll_gyro_dir As Byte Dim Nick_gyro_dir As Byte Dim Yaw_gyro_dir As Byte Dim Xacc_dir As Byte Dim Yacc_dir As Byte Dim P_sens_acro As Single Dim I_sens_acro As Single Dim P_sens_hover As Single Dim I_sens_hover As Single Dim D_sens_hover As Single Dim Yaw_p_sens_eep As Single Dim Yaw_i_sens_eep As Single Dim Acc_influence As Single Dim Gyro_influence As Single Dim Xacc_scale As Integer Dim Yacc_scale As Integer Dim Lf_acro As Single Dim Lf_hover As Single Dim Lf_yaw As Integer Dim Lf_boost As Byte Dim Idle_up As Word Dim Voltage_warn As Single Dim Xacc_offset As Integer Dim Yacc_offset As Integer Dim Meas_roll_gui As Single Dim Meas_nick_gui As Single Dim Yaw_gyro_gui As Single Dim Xacc_gui As Single Dim Yacc_gui As Single Dim Meas_angle_roll_gui As Single Dim Meas_angle_nick_gui As Single Dim Sempf_gui(5) As Single Dim Voltage_gui As Single Dim Throttlechannel As Byte Dim Nickchannel As Byte Dim Rollchannel As Byte Dim Yawchannel As Byte '--Subs-- Declare Sub Mixer 'prepare all signals for motors and servo Declare Sub Gyro 'read and prepare gyro's Declare Sub Acc 'read and rescale accelerometer Declare Sub Led 'bling bling Declare Sub Send_mots 'check and reshape motor signals and send them via i2c Declare Sub Voltage 'check voltage Declare Sub Failsave 'safety: turn off motors if reception is lost Declare Sub Guiconnection Declare Sub Geteeprom '--Alias's Led_1 Alias Portd.7 Led_2 Alias Portb.0 Led_3 Alias Portb.1 Led_grn Alias Portb.5 'a green led on the arduino mini pro '===START CONDITIONS=== Set Led_1 'light up led's after reset Set Led_2 Set Led_3 '--Variables-- 'prepare some variables Motors_on = 0 State = 0 'States: 0=GUI, 1=Acro, 2=Hover Ledcount = 1 Servotail = 62535 'neutral position for the servo Gyro_i_enable = 0 Clear Serialin Clear Serialout '--Startup 2-- Wait 1 Enable Timer0 Enable Timer1 Enable Interrupts Enable Int1 Start Adc I2cinit Ms_h = 0 : I2csend M_h , Ms_h 'turn off the motors Ms_r = 0 : I2csend M_r , Ms_r Ms_l = 0 : I2csend M_l , Ms_l Led_grn = 0 Waitms 500 Reset Led_1 'turn off led's Reset Led_2 Reset Led_3 '--Load and process settings from eeprom-- For I = 1 To 6 Toggle Led_grn Waitms 50 Next Geteeprom For I = 1 To 6 Toggle Led_grn Waitms 50 Next '===RC turned on...?=== Rc_test: If Motorsenable = 1 Then 'if motors are enabled from GUI, it is important to have stick in correct position 'switch (channel5) and throttle stick must be zero in order to proceed If Empf(5) > 80 Or Empf(5) < 50 Or Empf(throttlechannel) > 80 Or Empf(throttlechannel) < 50 Then 'sticks not in correct position If Rc_on_counter > 0 Then Decr Rc_on_counter End If Else 'sticks in correct position Incr Rc_on_counter End If If Rc_on_counter = 0 Then 'sticks not in correct position Toggle Led_1 'give signal to pilot Toggle Led_3 Waitms 75 End If If Rc_on_counter < 500 Then 'if we didn't get the correct stick position 500 times... Waitms 1 Goto Rc_test '... return and check again End If End If 'mot_en=1 Wait 1 'If we got the correct stick positions 500 times... proceed Led_grn = 1 Reset Led_1 Reset Led_3 '=== Measure Gyro Offset (don't move copter now!) === Roll_init = 0 Nick_init = 0 Yaw_init = 0 For I = 1 To 100 'read and sum up gyro's 100 times Roll_init = Roll_init + Getadc(3) Nick_init = Nick_init + Getadc(2) Yaw_init = Yaw_init + Getadc(1) Spann = Getadc(0) Toggle Led_grn If I < 33 Then 'bling bling Toggle Led_1 End If If I > 33 And I < 66 Then Reset Led_1 Toggle Led_2 End If If I > 66then Reset Led_2 Toggle Led_3 End If Waitms 13 Next Reset Led_1 Reset Led_2 Reset Led_3 Roll_init = Roll_init / 100 'get the average of the last 100 readings Nick_init = Nick_init / 100 Yaw_init = Yaw_init / 100 'Prepare receiver noise filter... Meanrx(1) = 63 Meanrx(2) = 100 Meanrx(3) = 100 Meanrx(4) = 100 Meanrx(5) = 63 '==============MAIN LOOP======================================================== 'All this does is calling subs in a loop all the time Do Acc Gyro Mixer Send_mots Led Voltage Failsave Guiconnection Loop End '===GYRO======================================================================== Sub Gyro If Old_state <> State Then 'compare state of last cycle with current state Error_roll = 0 'otherwise things would start mixing up... Error_nick = 0 Meas_roll = 0 Meas_nick = 0 Error_roll_sum = 0 Error_nick_sum = 0 If State = 2 Then Meas_angle_roll = Yacc 'when switching from acro mode to hover mode Meas_angle_nick = Xacc 'start with a "close to reality" angle Else Meas_angle_roll = 0 Meas_angle_nick = 0 End If Yaw_gyro_i = 0 Yaw_diff = 0 End If Old_state = State If Newvalsreceived = 1 Then Newvalsreceived = 0 Nickstickvel = Sempf(nickchannel) - Sempf_old_nick Sempf_old_nick = Sempf(nickchannel) Rollstickvel = Sempf(rollchannel) - Sempf_old_roll Sempf_old_roll = Sempf(rollchannel) If State = 1 Then Nickstickvel = Nickstickvel * 3 Rollstickvel = Rollstickvel * 3 Else Nickstickvel = Nickstickvel * 7 Rollstickvel = Rollstickvel * 7 End If End If If State = 1 Then 'ACROBATIC MODE = Angular velocity control If Looper < 2 Then 'for angular acceleration measurement Looper = Looper + 1 'acceleration will be calculated as the difference in velocity between Else 'loop n and loop n+2 Looper = 0 End If '--Roll-- Meas_roll = Getadc(3) 'get gyro signal If Roll_gyro_dir = 0 Then 'make gyro direction reversal possible Meas_roll = Roll_init - Meas_roll 'subtract offset Elseif Roll_gyro_dir = 1 Then Meas_roll = Meas_roll - Roll_init 'subtract offset End If Meas_roll = Meas_roll - Rollstickvel Setpoint_roll = Sempf(rollchannel) * Lfdynamic_roll 'sempf(2) is roll stick position Error_roll = Meas_roll - Setpoint_roll 'calculate difference between angular velocity and stick position If Looper = 0 Then 'this calculates the angular velocity (D-term in acro mode) Error_roll_d(1) = Error_roll - Error_roll_old(1) Error_roll_old(1) = Error_roll D_set_roll = Error_roll_d(1) * D_sens Elseif Looper = 1 Then Error_roll_d(2) = Error_roll - Error_roll_old(2) Error_roll_old(2) = Error_roll D_set_roll = Error_roll_d(2) * D_sens Elseif Looper = 2 Then Error_roll_d(3) = Error_roll - Error_roll_old(3) Error_roll_old(3) = Error_roll D_set_roll = Error_roll_d(3) * D_sens End If 'clip here: Error_roll_sum = Error_roll_sum + Error_roll 'integrate the above If Error_roll_sum > 10000 Then Error_roll_sum = 10000 End If If Error_roll_sum < -10000 Then Error_roll_sum = -10000 End If P_set_roll = Error_roll * P_sens 'multiply with gain If Gyro_i_enable = 0 Then 'don't integrate when motors off Error_roll_sum = 0 End If I_set_roll = Error_roll_sum * I_sens 'multiply with gain '--Nick-- Meas_nick = Getadc(2) 'see above If Nick_gyro_dir = 0 Then Meas_nick = Meas_nick - Nick_init Elseif Nick_gyro_dir = 1 Then Meas_nick = Nick_init - Meas_nick End If Meas_nick = Meas_nick - Nickstickvel Setpoint_nick = Sempf(nickchannel) * Lfdynamic_nick Error_nick = Meas_nick - Setpoint_nick If Looper = 0 Then 'this calculates the angular velocity (D-term in acro mode) Error_nick_d(1) = Error_nick - Error_nick_old(1) Error_nick_old(1) = Error_nick D_set_nick = Error_nick_d(1) * D_sens Elseif Looper = 1 Then Error_nick_d(2) = Error_nick - Error_nick_old(2) Error_nick_old(2) = Error_nick D_set_nick = Error_nick_d(2) * D_sens Elseif Looper = 2 Then Error_nick_d(3) = Error_nick - Error_nick_old(3) Error_nick_old(3) = Error_nick D_set_nick = Error_nick_d(3) * D_sens End If 'clip here: Error_nick_sum = Error_nick_sum + Error_nick If Error_nick_sum > 10000 Then Error_nick_sum = 10000 End If If Error_nick_sum < -10000 Then Error_nick_sum = -10000 End If P_set_nick = Error_nick * P_sens If Gyro_i_enable = 0 Then Error_nick_sum = 0 End If I_set_nick = Error_nick_sum * I_sens End If If State = 2 Or State = 0 Then 'HOVER MODE = Angle control If Looper = 0 Then 'for angular acceleration measurement Looper = 1 'acceleration will be calculated as the difference in velocity between Else 'loop n and loop n+2 Looper = 0 End If '--Roll-- Meas_roll = Getadc(3) 'get gyro signal If Roll_gyro_dir = 0 Then 'make gyro direction reversal possible Meas_roll = Roll_init - Meas_roll 'subtract offset Elseif Roll_gyro_dir = 1 Then Meas_roll = Meas_roll - Roll_init End If Meas_roll = Meas_roll - Rollstickvel If Looper = 0 Then Error_roll_d(1) = Meas_roll - Error_roll_old(1) Error_roll_old(1) = Meas_roll Dd_set_roll = Error_roll_d(1) * Dd_sens Elseif Looper = 1 Then Error_roll_d(2) = Meas_roll - Error_roll_old(2) Error_roll_old(2) = Meas_roll Dd_set_roll = Error_roll_d(2) * Dd_sens End If Meas_angle_roll = Meas_angle_roll + Meas_roll 'integrate gyro signal 'this might require some further explanation: 'The gyroscopes can only measure differences in rotational speed. Integrated over a long time (e.g. 11 minutes of flight) 'the angle calculated from gyro's alone loses precision. Here, the acc comes into play: It always knows the angle of the 'copter, but it reacts pretty slowly. And it contains quite some noise. The following lines of code combine the fast 'signal of the gyroscopes and the absolute precision of the accelerometer. In the end, you get the best out of both worlds: Meas_angle_roll = Meas_angle_roll * Gyro_influence '0.99 'take 0.99 of gyro integral and 0.01 of acc... Accy_temp = Yacc * Acc_influence '0.01 Meas_angle_roll = Meas_angle_roll + Accy_temp '...and put these two together (complementary filtering) Setpoint_roll = Sempf(rollchannel) * Lf 'roll stick position * stick sensitivity Error_roll = Meas_angle_roll - Setpoint_roll 'current angle minus desired angle (stick position) Error_roll_sum = Error_roll_sum + Error_roll 'integral of an integral If Error_roll_sum > 4000000 Then 'integral clipping Error_roll_sum = 4000000 End If If Error_roll_sum < -4000000 Then Error_roll_sum = -4000000 End If P_set_roll = Error_roll * P_sens 'multiply with gain If Gyro_i_enable = 0 Then Error_roll_sum = 0 End If I_set_roll = Error_roll_sum * I_sens 'multiply with gain D_set_roll = Meas_roll * D_sens 'multiply with gain '--Nick-- Meas_nick = Getadc(2) 'see above If Nick_gyro_dir = 0 Then Meas_nick = Meas_nick - Nick_init Elseif Nick_gyro_dir = 1 Then Meas_nick = Nick_init - Meas_nick End If Meas_nick = Meas_nick - Nickstickvel If Looper = 0 Then Error_nick_d(1) = Meas_nick - Error_nick_old(1) Error_nick_old(1) = Meas_nick Dd_set_nick = Error_nick_d(1) * Dd_sens Elseif Looper = 1 Then Error_nick_d(2) = Meas_nick - Error_nick_old(2) Error_nick_old(2) = Meas_nick Dd_set_nick = Error_nick_d(2) * Dd_sens End If Meas_angle_nick = Meas_angle_nick + Meas_nick Meas_angle_nick = Meas_angle_nick * Gyro_influence '0.99 Accx_temp = Xacc * Acc_influence '0.01 Meas_angle_nick = Meas_angle_nick + Accx_temp Setpoint_nick = Sempf(nickchannel) * Lf Error_nick = Meas_angle_nick - Setpoint_nick Error_nick_sum = Error_nick_sum + Error_nick If Error_nick_sum > 4000000 Then 'integral clipping Error_nick_sum = 4000000 End If If Error_nick_sum < -4000000 Then Error_nick_sum = -4000000 End If P_set_nick = Error_nick * P_sens If Gyro_i_enable = 0 Then Error_nick_sum = 0 End If I_set_nick = Error_nick_sum * I_sens D_set_nick = Meas_nick * D_sens End If 'convert rescaled values (single) to integers. P_set_roll_int = P_set_roll I_set_roll_int = I_set_roll D_set_roll_int = D_set_roll P_set_nick_int = P_set_nick I_set_nick_int = I_set_nick D_set_nick_int = D_set_nick Dd_set_nick_int = Dd_set_nick Dd_set_roll_int = Dd_set_roll '--Yaw-- Yaw_gyro = Getadc(1) 'get yaw rate from gyro If Yaw_gyro_dir = 0 Then Yaw_gyro = Yaw_gyro - Yaw_init 'subtract offset Elseif Yaw_gyro_dir = 1 Then Yaw_gyro = Yaw_init - Yaw_gyro End If Yaw_diff = Sempf(yawchannel) * Lf_yaw 'yaw stick position Yaw_diff = Yaw_diff - Yaw_gyro 'stick position - current angular velocity Yaw_gyro_i = Yaw_gyro_i + Yaw_diff 'integral of the above If Yaw_gyro_i > 32000 Then 'protect from overflow Yaw_gyro_i = 32000 End If If Yaw_gyro_i < -32000 Then 'protect from overflow Yaw_gyro_i = -32000 End If Yaw_gyro_scale = Yaw_diff * Yaw_p_sens 'multiply with gain If Gyro_i_enable = 0 Then 'integrate only when motors on Yaw_gyro_i = 0 End If Yaw_gyro_i_scale = Yaw_gyro_i * Yaw_i_sens 'multiply with gain Yaw_gyro_scale_int = Yaw_gyro_scale 'convert single to integer Yaw_gyro_i_scale_int = Yaw_gyro_i_scale 'convert single to integer End Sub '===ACCELEROMETER=============================================================== Sub Acc If State <> 1 Then 'only used in HOVER MODE and when motors off Xacc = Getadc(4) 'read nick "angle" Yacc = Getadc(5) 'read roll "angle" If Xacc_dir = 1 Then 'make xacc direction reversable Xacc = Xacc_offset - Xacc '515 'these values are the ACC offsets. They were determined in flight (hovering in place) Elseif Xacc_dir = 0 Then Xacc = Xacc - Xacc_offset End If If Yacc_dir = 1 Then Yacc = Yacc_offset - Yacc '524 Elseif Yacc_dir = 0 Then Yacc = Yacc - Yacc_offset End If Yacc = Yacc * Yacc_scale '120 'make the amplitude if gyro_integrals and acc similar Xacc = Xacc * Xacc_scale '120 ' (necessary for successful complementary filtering) End If End Sub '===MIXER======================================================================= Sub Mixer '--Noise filter-- For I = 1 To 5 Meanrx(i) = Meanrx(i) * 3 Meanrx(i) = Meanrx(i) + Empf(i) '"lowpass filter" of the RC signal Shift Meanrx(i) , Right , 2 ' (=divide by 4) Aempfh(i) = Meanrx(i) + 17 'upper acceptable fluctuation Aempfl(i) = Meanrx(i) - 17 'lower acceptable fluctuation If Empf(i) > Aempfh(i) Or Empf(i) < Aempfl(i) Then 'compare allowed fluctuation with current rc reading Empf(i) = Meanrx(i) 'if fluctuation was too high -> replace with averaged value Print "Error reading channel: " ; I 'handy to check for RC problems End If Next 'Empf(X) are filled in "getreceiver". They usually contain values ranging from 63 - 137. 'Empf(throttlechannel) is the throttle stick. it will be rescaled differently. If Empf(throttlechannel) > 61 And Empf(throttlechannel) < 139 Then 'don't process values that can't be correct Sempf(throttlechannel) = Empf(throttlechannel) - 61 Sempf(throttlechannel) = Sempf(throttlechannel) * 3 '==> values ranging from 3 (stick at bottom) to 228 (full throttle) End If 'Now nick, roll, yaw and idle up switch If Empf(nickchannel) > 61 And Empf(nickchannel) < 139 Then 'don't process values that can't be correct Sempf(nickchannel) = Empf(nickchannel) - 100 'convert to values ranging from -37 to +37 End If If Empf(rollchannel) > 61 And Empf(rollchannel) < 139 Then 'don't process values that can't be correct Sempf(rollchannel) = Empf(rollchannel) - 100 'convert to values ranging from -37 to +37 End If If Empf(yawchannel) > 61 And Empf(yawchannel) < 139 Then 'don't process values that can't be correct Sempf(yawchannel) = Empf(yawchannel) - 100 'convert to values ranging from -37 to +37 End If If Empf(5) > 61 And Empf(5) < 139 Then 'don't process values that can't be correct Sempf(5) = Empf(5) - 100 'convert to values ranging from -37 to +37 End If 'Dynamic LF: Makes the copter react non-linearly to nick. I use this for flying loopings. The more I pull, the faster(exponential) the copter will nick If Lf_boost = 1 Then If State = 1 Then 'only when in acro mode Lookup_pos_nick = Abs(sempf(nickchannel)) 'make a variable that grows when stick is out of centre Lookup_pos_nick = Lookup_pos_nick - 25 If Lookup_pos_nick < 0 Then Lookup_pos_nick = 0 End If If Lookup_pos_nick >= 13 Then Lookup_pos_nick = 13 End If If Lookup_pos_nick <> 0 Then Lfdynamic_nick = Lookup(lookup_pos_nick , Dta) 'look for a new sensitivity factor in a table Else Lfdynamic_nick = Lf End If Lookup_pos_roll = Abs(sempf(rollchannel)) 'make a variable that grows when stick is out of centre Lookup_pos_roll = Lookup_pos_roll - 26 If Lookup_pos_roll < 0 Then Lookup_pos_roll = 0 End If If Lookup_pos_roll >= 13 Then Lookup_pos_roll = 13 End If If Lookup_pos_roll <> 0 Then Lfdynamic_roll = Lookup(lookup_pos_roll , Dta) 'look for a new sensitivity factor in a table Else Lfdynamic_roll = Lf End If End If Else Lfdynamic_roll = Lf Lfdynamic_nick = Lf End If 'ACRO MODE (angular velocity control, ACC = off) If Motorsenable = 1 Then 'only listen to receiver (and especially channel 5) when user enabled the motors in the GUI If Sempf(5) > -7 And Sempf(5) < 7 Then 'switch in middle = motors on; sempf(5) is the idle up switch Vorwahl = Idle_up 'minimum throttle Gyro_i_enable = 1 'start integrating the gyroscope signals Lf = Lf_acro '5.8 'nick and roll sensitivity P_sens = P_sens_acro '0.45 'Gyro_P sensitivity roll and nick I_sens = I_sens_acro '0.001 'Gyro_I Sensitivity roll and nick D_sens = D_sens_acro 'no D available in Acro mode (calculating it only yields noise) Yaw_p_sens = Yaw_p_sens_eep 'Gyro_P Sensitivity yaw Yaw_i_sens = Yaw_i_sens_eep 'Gyro_I Sensitivity yaw Motors_on = 1 State = 1 'flight mode: acrobatic End If 'HOVER MODE (angle control, ACC = on) If Sempf(5) >= 20 And Sempf(5) < 39 Then 'switch top = motors on Vorwahl = Idle_up 'minimum throttle Gyro_i_enable = 1 'start integrating the gyroscope signals Lf = Lf_hover '450 'nick and roll sensitivity (much bigger, because a different control loop is used) P_sens = P_sens_hover '0.0025 'Gyro_P sensitivity roll and nick (corresponds to Gyro_I in acro mode) I_sens = I_sens_hover '0.0000015 'Gyro_I sensitivity roll and nick (this is a double integral, thats why the value is so low) D_sens = D_sens_hover '0.4 'Gyro_D sensitivity roll and nick (corresponds to Gyro_P in acro mode) Yaw_p_sens = Yaw_p_sens_eep 'Gyro_P Sensitivity yaw Yaw_i_sens = Yaw_i_sens_eep 'Gyro_I Sensitivity yaw Motors_on = 1 State = 2 'flight mode: hover End If If Sempf(5) <= -20 And Sempf(5) > -39 Then 'switch bottom = motors off If Sempf(throttlechannel) < 50 Then 'only turn motors off when throttle stick is also on bottom Vorwahl = 0 'no minimum throttle = motors off Gyro_i_enable = 0 'do not integrate gyros anymore Motors_on = 0 State = 0 'flight mode: off Lf = 0 'don't react to stick movements 'Lf_tail = 0 P_sens = 0 'don't react to gyros I_sens = 0 'don't react to gyros D_sens = 0 'don't react to gyros Yaw_p_sens = 0 'don't react to gyros Yaw_i_sens = 0 'don't react to gyros End If End If Else 'if motors were not enabled in GUI or if EEprom emty: always stay in GUI mode Vorwahl = 0 'no minimum throttle = motors off Gyro_i_enable = 0 'do not integrate gyros anymore Motors_on = 0 State = 0 'flight mode: off Lf = 0 'don't react to stick movements 'Lf_tail = 0 P_sens = 0 'don't react to gyros I_sens = 0 'don't react to gyros D_sens = 0 'don't react to gyros Yaw_p_sens = 0 'don't react to gyros Yaw_i_sens = 0 'don't react to gyros End If '-Mix components- If Motors_on = 1 Then 'only when motors are running '-MOTOR 1- (front left) Ms_l_i = Sempf(throttlechannel) + Vorwahl 'throttle stick (3-228) + Vorwahl(20) If Ms_l_i > 247 Then 'throttle clipping Ms_l_i = 247 End If Ms_l_i = Ms_l_i + P_set_roll_int 'add the gyro measurements (note the differences in "+" and "-") Ms_l_i = Ms_l_i + P_set_nick_int Ms_l_i = Ms_l_i + I_set_roll_int Ms_l_i = Ms_l_i + I_set_nick_int Ms_l_i = Ms_l_i + D_set_roll_int Ms_l_i = Ms_l_i + D_set_nick_int If State = 2 Then 'add DD term in hover mode Ms_l_i = Ms_l_i + Dd_set_roll_int Ms_l_i = Ms_l_i + Dd_set_nick_int End If '-MOTOR 2- (front right) Ms_r_i = Sempf(throttlechannel) + Vorwahl If Ms_r_i > 247 Then Ms_r_i = 247 End If Ms_r_i = Ms_r_i - P_set_roll_int Ms_r_i = Ms_r_i + P_set_nick_int Ms_r_i = Ms_r_i - I_set_roll_int Ms_r_i = Ms_r_i + I_set_nick_int Ms_r_i = Ms_r_i - D_set_roll_int Ms_r_i = Ms_r_i + D_set_nick_int If State = 2 Then Ms_r_i = Ms_r_i - Dd_set_roll_int Ms_r_i = Ms_r_i + Dd_set_nick_int End If '-MOTOR 3- (back) If Ms_h_i > 247 Then Ms_h_i = 247 End If Ms_h_i = Sempf(throttlechannel) + Vorwahl Ms_h_i = Ms_h_i - P_set_nick_int Ms_h_i = Ms_h_i - I_set_nick_int Ms_h_i = Ms_h_i - D_set_nick_int If State = 2 Then Ms_h_i = Ms_h_i - Dd_set_nick_int End If 'when yawing very fast, throttle for the motor at the back is increased Tailgas = Yaw_gyro_scale_int + Yaw_gyro_i_scale_int 'if these values are big, the motor at the back was tilted Tailgas = Abs(tailgas) / 100 Ms_h_i = Ms_h_i + Tailgas Else 'motors off Ms_l_i = 0 Ms_r_i = 0 Ms_h_i = 0 End If '-Servo control- 'Servo neutral= 62535. First I add 32535, then I again add 30000 (=62535). This makes sense because I want 'to protect "Servotail" from overflow. If Shigh = 0 Then 'new values will be assigned only with the speed that the servo supports. Servotail = 32535 + Yaw_gyro_scale_int 'add gyro_P Servotail = Servotail + Yaw_gyro_i_scale_int 'add gyro_I If Servotail > 33534 Then 'Overflow Protection Servotail = 33534 End If If Servotail < 31536 Then 'Overflow Protection Servotail = 31536 End If Servotail = Servotail + 30000 'add the remaining 30000 for neutral position End If End Sub '===MOTORS I2C================================================================== Sub Send_mots If Ms_l_i > 255 Then 'cap motor signals (upper limit = 255) Ms_l_i = 255 End If If Motors_on = 1 Then If Ms_l_i < Vorwahl Then 'cap motor signals (lower limit = vorwahl) Ms_l_i = Vorwahl End If End If If Ms_l_i < 0 Then 'cap motor signals (lower limit = 0) Ms_l_i = 0 End If If Ms_r_i > 255 Then 'cap motor signals (upper limit = 255) Ms_r_i = 255 End If If Motors_on = 1 Then If Ms_r_i < Vorwahl Then 'cap motor signals (lower limit = vorwahl) Ms_r_i = Vorwahl End If End If If Ms_r_i < 0 Then 'cap motor signals (lower limit = 0) Ms_r_i = 0 End If If Ms_h_i > 255 Then 'cap motor signals (upper limit = 255) Ms_h_i = 255 End If If Motors_on = 1 Then If Ms_h_i < Vorwahl Then 'cap motor signals (lower limit = vorwahl) Ms_h_i = Vorwahl End If End If If Ms_h_i < 0 Then 'cap motor signals (lower limit = 0) Ms_h_i = 0 End If Ms_l = Ms_l_i 'convert integer to word Ms_h = Ms_h_i Ms_r = Ms_r_i If Failure < 15 And Motorsenable = 1 Then 'if there are NO problems with the receiver and shrediquette was programmed: run motors I2csend M_h , Ms_h I2csend M_r , Ms_r I2csend M_l , Ms_l Else 'if there are problems with the receiver: turn off motors I2csend M_h , 0 I2csend M_r , 0 I2csend M_l , 0 End If End Sub '===LEDs======================================================================== Sub Led If Ledcount < 500 Then Incr Ledcount If Lowvoltage = 0 Then If State > 0 Then 'flight modes 1 & 2 get a different LED signal If Ledcount = 130 Or Ledcount = 140 Or Ledcount = 230 Or Ledcount = 240 Then 'flash LEDs Toggle Led_1 End If If Ledcount = 100 Or Ledcount = 110 Or Ledcount = 200 Or Ledcount = 210 Then Toggle Led_2 End If If Ledcount = 160 Or Ledcount = 170 Or Ledcount = 260 Or Ledcount = 270 Then Toggle Led_3 End If If Ledcount = 470 Then Toggle Led_grn End If Else 'state = 0 ==> ready for GUI connection If Ledcount = 100 Or Ledcount = 120 Or Ledcount = 200 Or Ledcount = 220 Or Ledcount = 300 Or Ledcount = 320 Then Toggle Led_1 Toggle Led_2 Toggle Led_3 Toggle Led_grn End If End If End If If Lowvoltage = 1 Then 'when battery is low, blink the LEDs in an annoying manner If Blinker < 30 Then Incr Blinker Else Blinker = 1 Toggle Led_1 Toggle Led_2 Toggle Led_3 End If Reset Led_grn End If Else If State > 0 Then 'ledcount> 500 Set Led_grn 'toggle green led every 500 cycles (I use this to determine the speed of the program) Else Reset Led_grn End If Ledcount = 1 Reset Led_1 Reset Led_2 Reset Led_3 End If End Sub '===VOLTAGE===================================================================== Sub Voltage If Ledcount = 500 Then 'we'll only check voltage every 500 cycles (==> about 1 Hz) Spann = Getadc(0) Spannmittel = Spann + Spann_alt 'very unprofessional moving average filter Spannmittel = Spannmittel + Spann_alt2 Spannmittel = Spannmittel + Spann_alt3 Spannmittel = Spannmittel / 4 Spann_alt = Spann Spann_alt2 = Spann_alt Spann_alt3 = Spann_alt2 'ADC = 80.775 * voltage - 4.783 'experimentally determinded scale factor of the ADC ' 10,50 V -> 843 ' 10,40 V -> 835 ' 10,30 V -> 827 ' 10,20 V -> 819 ' 10,10 V -> 811 ' 10,00 V -> 803 ' 9,90 V -> 795 ' 9,80 V -> 787 ' 9,70 V -> 779 ' 9,60 V -> 771 ' 9,50 V -> 763 ' 9,40 V -> 755 ' 9,30 V -> 746 ' 9,20 V -> 738 If Spannmittel < Voltage_warn Then '795 => 9.9V Lowvoltage = 1 'set low-voltage bit when voltage is low End If End If End Sub '===FAILSAVE==================================================================== Sub Failsave 'will be used to shut down motors if receiption is bad. If Channel >= 11 Then '"channel" will be greater than 11 if something with the receiver went wrong If Failure < 255 Then Incr Failure 'If this happens: increase Failure End If End If If Channel < 11 Then '"Channel" will be lower than 11 if rx experiences NO problems If Failure > 0 Then Decr Failure 'Failure will accordingly be decreased End If End If End Sub '===GUI========================================================================= Sub Guiconnection 'RS232 link to the GUI If State = 0 Then 'don't listen to serial commands when motors running! If Ischarwaiting() > 0 Then Input A Noecho Read_empfangen = Instr(a , "cr!") Sensors_empfangen = Instr(a , "cs!") Input_empfangen = Instr(a , "ci!") Reset_empfangen = Instr(a , "reset!") If Reset_empfangen = 1 Then Reset_empfangen = 0 Waitms 100 Goto $3c00 '$3c00 -> m328p; $1c00 -> m168 End If If Read_empfangen = 1 Then Read_empfangen = 0 Waitms 25 Print "s#p!" 'tell pc that data will follow Printbin Var(1) ; 33 'print the list of parameters that can be modified End If If Sensors_empfangen = 1 Then Sensors_empfangen = 0 Print "ss!" 'tell pc that sensor data will follow 'rescale the readings from the sensors to a value ranging from 0 to 255 ' and put these values in an array "sensor(X)" Meas_roll_gui = Meas_roll / 2 If Meas_roll_gui > 127 Then Meas_roll_gui = 127 End If If Meas_roll_gui < -127 Then Meas_roll_gui = -127 End If Meas_roll_gui = Meas_roll_gui + 127 Sensor(1) = Meas_roll_gui Meas_angle_roll_gui = Meas_angle_roll / 200 If Meas_angle_roll_gui > 127 Then Meas_angle_roll_gui = 127 End If If Meas_angle_roll_gui < -127 Then Meas_angle_roll_gui = -127 End If Meas_angle_roll_gui = Meas_angle_roll_gui + 127 Sensor(2) = Meas_angle_roll_gui Yacc_gui = Yacc / 200 If Yacc_gui > 127 Then Yacc_gui = 127 End If If Yacc_gui < -127 Then Yacc_gui = -127 End If Yacc_gui = Yacc_gui + 127 Sensor(3) = Yacc_gui Meas_nick_gui = Meas_nick / 2 If Meas_nick_gui > 127 Then Meas_nick_gui = 127 End If If Meas_nick_gui < -127 Then Meas_nick_gui = -127 End If Meas_nick_gui = Meas_nick_gui + 127 Sensor(4) = Meas_nick_gui Meas_angle_nick_gui = Meas_angle_nick / 200 If Meas_angle_nick_gui > 127 Then Meas_angle_nick_gui = 127 End If If Meas_angle_nick_gui < -127 Then Meas_angle_nick_gui = -127 End If Meas_angle_nick_gui = Meas_angle_nick_gui + 127 Sensor(5) = Meas_angle_nick_gui Xacc_gui = Xacc / 200 If Xacc_gui > 127 Then Xacc_gui = 127 End If If Xacc_gui < -127 Then Xacc_gui = -127 End If Xacc_gui = Xacc_gui + 127 Sensor(6) = Xacc_gui Yaw_gyro_gui = Yaw_gyro / 2 If Yaw_gyro_gui > 127 Then Yaw_gyro_gui = 127 End If If Yaw_gyro_gui < -127 Then Yaw_gyro_gui = -127 End If Yaw_gyro_gui = Yaw_gyro_gui + 127 Sensor(7) = Yaw_gyro_gui Sempf_gui(1) = Sempf(throttlechannel) * 1.1 Sensor(8) = Sempf_gui(1) Sempf_gui(2) = Sempf(rollchannel) + 39 Sempf_gui(2) = Sempf_gui(2) * 3.2 Sensor(9) = Sempf_gui(2) Sempf_gui(3) = Sempf(nickchannel) + 39 Sempf_gui(3) = Sempf_gui(3) * 3.2 Sensor(10) = Sempf_gui(3) Sempf_gui(4) = Sempf(yawchannel) + 39 Sempf_gui(4) = Sempf_gui(4) * 3.2 Sensor(11) = Sempf_gui(4) Sempf_gui(5) = Sempf(5) + 39 Sempf_gui(5) = Sempf_gui(5) * 3.2 Sensor(12) = Sempf_gui(5) Voltage_gui = Spannmittel / 2 Voltage_gui = Voltage_gui - 256 Sensor(13) = Voltage_gui Printbin Sensor(1) ; 13 End If If Input_empfangen = 1 Then 'pc wants to transfer parameters Input_empfangen = 0 Inputbin Var(1) , 33 'read list of parameters 'Ms_h = 0 : I2csend M_h , Ms_h 'turn off the motors 'Ms_r = 0 : I2csend M_r , Ms_r 'Ms_l = 0 : I2csend M_l , Ms_l Waitms 10 For I = 1 To 33 Writeeeprom Var(i) , I 'save parameters to eeprom Next Waitms 50 Geteeprom End If End If End If End Sub '===READ AND PROCESS EEPROM===================================================== Sub Geteeprom Ms_h = 0 : I2csend M_h , Ms_h 'turn off the motors Ms_r = 0 : I2csend M_r , Ms_r Ms_l = 0 : I2csend M_l , Ms_l 'load and convert settings from eeprom For I = 1 To 33 Readeeprom Var(i) , I Next Convert_vars: Motorsenable = Var(1) Roll_gyro_dir = Var(2) Nick_gyro_dir = Var(3) Yaw_gyro_dir = Var(4) Xacc_dir = Var(5) Yacc_dir = Var(6) P_sens_acro = Var(7) / 255 I_sens_acro = Var(8) / 25500 P_sens_hover = Var(9) / 25500 I_sens_hover = Var(10) / 25500000 'decrease factor to 12800000 D_sens_hover = Var(11) / 255 Yaw_p_sens_eep = Var(12) / 21 Yaw_i_sens_eep = Var(13) / 2550 Acc_influence = Var(14) / 3000 Gyro_influence = 1 - Acc_influence Xacc_scale = Var(15) Yacc_scale = Var(16) Lf_acro = Var(17) / 25.5 Lf_hover = Var(18) * 4 Lf_yaw = Var(19) / 17 Lf_boost = Var(20) Idle_up = Var(21) / 5 Voltage_warn = Var(22) * 4 Xacc_offset = Var(23) + 384 Yacc_offset = Var(24) + 384 Throttlechannel = Var(25) Nickchannel = Var(26) Rollchannel = Var(27) Yawchannel = Var(28) D_sens_acro = Var(29) / 50 Dd_sens = Var(30) / 50 'if eeprom is empty: vars will be 255. That can cause weird things ==> set all vars to 0 If Roll_gyro_dir > 100 Or Nick_gyro_dir > 100 Or Yaw_gyro_dir > 100 Or Xacc_dir > 100 Or Yacc_dir > 100 Or Lf_boost > 100 Or Throttlechannel > 4 Or Rollchannel > 4 Or Nickchannel > 4 Or Yawchannel > 4 Then For I = 1 To 33 Var(i) = 0 'set to zero Next Var(25) = 1 Var(26) = 3 Var(27) = 2 Var(28) = 4 Goto Convert_vars 'and reinitialize the variables End If End Sub '===SERVO CONTROL=============================================================== Servoirq: 'generate servo PWM pulse If Shigh = 0 Then Timer1 = Servotail Portd.6 = 1 Shigh = 1 Else Portd.6 = 0 Shigh = 0 '35535 Timer1 = 30000 'after a pulse, pause for 17ms (= 57Hz typically used for servos) End If Return '===READ RX===================================================================== Getreceiver: 'falling edge detection If Channel > 0 And Channel < 6 Then 'fill empf(1-5) Empf(channel) = Timer0 End If Timer0 = 6 'preload for 4.096 ms Incr Channel 'if no falling edge was detected for a longer period, channel will increase above 11 Return 'that means that there are problems with the receiver Detectrxpause: If Channel <> 0 Then Newvalsreceived = 1 End If Channel = 0 Return Dta: 'table lookup for dynamic lf (increasing maneuvrability when stick is out of the centre) Data 5.8! , 6.1! , 6.3! , 6.5! , 6.8! , 7.2! , 7.6! , 8.1! , 8.7! , 9.4! , 10.2! , 11! , 12! , 13!