main.py

"""
The Scout Flight Controller
by Tim Hanewich - github.com/TimHanewich
For more information: https://github.com/TimHanewich/scout

Copyright 2023 Tim Hanewich
Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
"""

########################################
###########  SETTINGS  #################
########################################

# Motor GPIO's (not pin number, GPIO number) ###
gpio_motor1 = 2 # front left, clockwise
gpio_motor2 = 28 # front right, counter clockwise
gpio_motor3 = 15 # rear left, counter clockwise
gpio_motor4 = 16 # rear right, clockwise

# i2c pins used for MPU-6050
gpio_i2c_sda = 12
gpio_i2c_scl = 13

# RC-receiver UART
# either a 0 or 1 (Raspberry Pi Pico supports two UART interfaces)
rc_uart = 1

# throttle settings
throttle_idle:float = 0.14 # the minumum throttle needed to apply to the four motors for them all to spin up, but not provide lift (idling on the ground). the only way to find this value is through testing (with props off).
throttle_governor:float = 0.22 # the maximum throttle that can be applied. So, if the pilot is inputting 100% on the controller, it will max out at this. And every value below the pilot's 100% will be scaled linearly within the range of the idle throttle seen above and this governor throttle. If you do not wish to apply a governor (not recommended), set this to None.

# Max attitude rate of change rates (degrees per second)
max_rate_roll:float = 30.0 # roll
max_rate_pitch:float = 30.0 # pitch
max_rate_yaw:float = 50.0 # yaw

# Desired Flight Controller Cycle time
# This is the number of times per second the flight controller will perform an adjustment loop (PID loop)
target_cycle_hz:float = 250.0

# PID Controller values
pid_roll_kp:float = 0.00043714285
pid_roll_ki:float = 0.00255
pid_roll_kd:float = 0.00002571429
pid_pitch_kp:float = pid_roll_kp
pid_pitch_ki:float = pid_roll_ki
pid_pitch_kd:float = pid_roll_kd
pid_yaw_kp:float = 0.001714287
pid_yaw_ki:float = 0.003428571
pid_yaw_kd:float = 0.0

########################################
########################################
########################################

import machine
import time
import ibus
import toolkit

# THE FLIGHT CONTROL LOOP
def run() -> None:
    
    print("Hello from Scout!")

    # flash the LED a few times to show the microcontroller has received power and the program is active
    led = machine.Pin(25, machine.Pin.OUT) # the onboard LED of the Raspberry Pi Pico
    for x in range(8):
        led.on()
        time.sleep(0.1)
        led.off()
        time.sleep(0.1)

    # wait a few seconds for the IMU to settle
    print("Waiting 3 seconds for the IMU to settle...")
    time.sleep(3)

    # overclock
    machine.freq(250000000)
    print("Overclocked to 250,000,000")

    # set up RC receiver
    rc:ibus.IBus = ibus.IBus(rc_uart)
    print("RC receiver set up")

    # check that flight mode is not on when we are first starting - it needs to be set into standby mode
    # this is a safety check. Prevents from the drone taking off (at least spinning props) as soon as power is plugged in
    print("Validating that mode switch is not in flight position...")
    for x in range(0, 60):
        rc_data = rc.read()
        if rc_data[5] == 2000: # flight mode on
            FATAL_ERROR("Flight mode detected as on (from RC transmitter) as soon as power was received. As a safety precaution, mode switch needs to be in standby mode when system is powered up.")
        time.sleep(0.025)

    # Print settings that are important
    print("Roll PID: " + str(pid_roll_kp) + ", " + str(pid_roll_ki) + ", " + str(pid_roll_kd))
    print("Pitch PID: " + str(pid_pitch_kp) + ", " + str(pid_pitch_ki) + ", " + str(pid_pitch_kd))
    print("Yaw PID: " + str(pid_yaw_kp) + ", " + str(pid_yaw_ki) + ", " + str(pid_yaw_kd))

    # Set up IMU (MPU-6050)
    i2c = machine.I2C(0, sda = machine.Pin(gpio_i2c_sda), scl = machine.Pin(gpio_i2c_scl))
    mpu6050_address:int = 0x68
    i2c.writeto_mem(mpu6050_address, 0x6B, bytes([0x01])) # wake it up
    i2c.writeto_mem(mpu6050_address, 0x1A, bytes([0x05])) # set low pass filter to 5 (0-6)
    i2c.writeto_mem(mpu6050_address, 0x1B, bytes([0x08])) # set gyro scale to 1 (0-3)

    # confirm IMU is set up
    whoami:int = i2c.readfrom_mem(mpu6050_address, 0x75, 1)[0]
    lpf:int = i2c.readfrom_mem(mpu6050_address, 0x1A, 1)[0]
    gs:int = i2c.readfrom_mem(mpu6050_address, 0x1B, 1)[0]
    
    # did who am I work?
    if whoami == 104: #0x68
        print("MPU-6050 WHOAMI validated!")
    else:
        FATAL_ERROR("ERROR! MPU-6050 WHOAMI failed! '" + str(whoami) + "' returned.")

    # did lpf get set?
    if lpf == 0x05:
        print("MPU-6050 LPF set to " + str(lpf) + " correctly.")
    else:
        FATAL_ERROR("ERROR! MPU-6050 LPF did not set correctly. Set to '" + str(lpf) + "'")

    # did gyro scale get set?
    if gs == 0x08:
        print("MPU-6050 Gyro Scale set to " + str(gs) + " correctly.")
    else:
        FATAL_ERROR("ERROR! MPU-6050 gyro scale did not set correctly. " + str(gs) + " returned.")

    # measure gyro bias
    print("Measuring gyro bias...")
    gxs:list[float] = []
    gys:list[float] = []
    gzs:list[float] = []
    started_at_ticks_ms:int = time.ticks_ms()
    while ((time.ticks_ms() - started_at_ticks_ms) / 1000) < 3.0:
        gyro_data = i2c.readfrom_mem(mpu6050_address, 0x43, 6) # read 6 bytes (2 for each axis)
        gyro_x = (translate_pair(gyro_data[0], gyro_data[1]) / 65.5)
        gyro_y = (translate_pair(gyro_data[2], gyro_data[3]) / 65.5)
        gyro_z = (translate_pair(gyro_data[4], gyro_data[5]) / 65.5) * -1 # multiply by -1 because of the way I have it mounted on the quadcopter - it may be upside down. I want a "yaw to the right" to be positive and a "yaw to the left" to be negative.
        gxs.append(gyro_x)
        gys.append(gyro_y)
        gzs.append(gyro_z)
        time.sleep(0.025)
    gyro_bias_x = sum(gxs) / len(gxs)
    gyro_bias_y = sum(gys) / len(gys)
    gyro_bias_z = sum(gzs) / len(gzs)
    print("Gyro bias: " + str((gyro_bias_x, gyro_bias_y, gyro_bias_z)))

    # Set up PWM's
    M1:machine.PWM = machine.PWM(machine.Pin(gpio_motor1))
    M2:machine.PWM = machine.PWM(machine.Pin(gpio_motor2))
    M3:machine.PWM = machine.PWM(machine.Pin(gpio_motor3))
    M4:machine.PWM = machine.PWM(machine.Pin(gpio_motor4))
    M1.freq(250)
    M2.freq(250)
    M3.freq(250)
    M4.freq(250)
    print("Motor PWM's set up @ 250 hz")

    # Constants calculations / state variables - no need to calculate these during the loop (save processor time)
    cycle_time_seconds:float = 1.0 / target_cycle_hz
    cycle_time_us:int = int(round(cycle_time_seconds * 1000000, 0)) # multiply by 1,000,000 to go from seconds to microseconds (us)
    max_throttle = throttle_governor if throttle_governor is not None else 1.0 # what is the MAX throttle we can apply (considering governor)?
    throttle_range:float = max_throttle - throttle_idle # used for adjusted throttle calculation
    i_limit:float = 150.0 # PID I-term limiter. The applied I-term cannot exceed or go below (negative) this value. (safety mechanism to prevent excessive spooling of the motors)
    last_mode:bool = False # the most recent mode the flight controller was in. False = Standby (props not spinning), True = Flight mode
    
    # State variables - PID related
    # required to be delcared outside of the loop because their state will be used in multiple loops (passed from loop to loop)
    roll_last_integral:float = 0.0
    roll_last_error:float = 0.0
    pitch_last_integral:float = 0.0
    pitch_last_error:float = 0.0
    yaw_last_integral:float = 0.0
    yaw_last_error:float = 0.0

    # INFINITE LOOP
    led.on() # turn on the onboard LED to signal that the flight controller is now active
    print("-- BEGINNING FLIGHT CONTROL LOOP NOW --")
    try:
        while True:
            
            # mark start time
            loop_begin_us:int = time.ticks_us()

            # Capture raw IMU data
            # we divide by 65.5 here because that is the modifier to use at a gyro range scale of 1, which we are using.
            gyro_data = i2c.readfrom_mem(mpu6050_address, 0x43, 6) # read 6 bytes (2 for each axis)
            gyro_x = ((translate_pair(gyro_data[0], gyro_data[1]) / 65.5) - gyro_bias_x) * -1 # Roll rate. we multiply by -1 here because of the way I have it mounted. it should be rotated 180 degrees I believe, but it's okay, I can flip it here. 
            gyro_y = (translate_pair(gyro_data[2], gyro_data[3]) / 65.5) - gyro_bias_y # Pitch rate.
            gyro_z = ((translate_pair(gyro_data[4], gyro_data[5]) / 65.5) * -1) - gyro_bias_z # Yaw rate. multiply by -1 because of the way I have it mounted - it may be upside down. I want a "yaw to the right" to be positive and a "yaw to the left" to be negative.

            # Read control commands from RC
            rc_data = rc.read()

            # normalize all RC input values
            input_throttle:float = normalize(rc_data[3], 1000.0, 2000.0, 0.0, 1.0) # between 0.0 and 1.0
            input_pitch:float = (normalize(rc_data[2], 1000.0, 2000.0, -1.0, 1.0)) * -1 # between -1.0 and 1.0. We multiply by -1 because... If the pitch is "full forward" (i.e. 75), that means we want a NEGATIVE pitch (when a plane pitches it's nose down, that is negative, not positive. And when a place pitches it's nose up, pulling back on the stick, it's positive, not negative.) Thus, we need to flip it.
            input_roll:float = normalize(rc_data[1], 1000.0, 2000.0, -1.0, 1.0) # between -1.0 and 1.0
            input_yaw:float = normalize(rc_data[4], 1000.0, 2000.0, -1.0, 1.0) # between -1.0 and 1.0

            # ADJUST MOTOR OUTPUTS!
            # based on channel 5. Channel 5 I have assigned to the switch that determines flight mode (standby/flight)
            if rc_data[5] == 1000: # standby mode - switch in "up" or OFF position
            
                # turn motors off completely
                duty_0_percent:int = calculate_duty_cycle(0.0)
                M1.duty_ns(duty_0_percent)
                M2.duty_ns(duty_0_percent)
                M3.duty_ns(duty_0_percent)
                M4.duty_ns(duty_0_percent)

                # reset PID's
                roll_last_integral = 0.0
                roll_last_error = 0.0
                pitch_last_integral = 0.0
                pitch_last_error = 0.0
                yaw_last_integral = 0.0
                yaw_last_error = 0.0

                # set last mode
                last_mode = False # False means standby mode

            elif rc_data[5] == 2000: # flight mode (idle props at least) - swith in "down" or ON position

                # if last mode was standby (we JUST were turned onto flight mode), perform a check that the throttle isn't high. This is a safety mechanism
                # this prevents an accident where the flight mode switch is turned on but the throttle position is high, which would immediately apply heavy throttle to each motor, shooting it into the air.
                if last_mode == False: # last mode we were in was standby mode. So, this is the first frame we are going into flight mode
                    if input_throttle > 0.05: # if throttle is > 5%
                        FATAL_ERROR("Throttle was set to " + str(input_throttle) + " as soon as flight mode was entered. Throttle must be at 0% when flight mode begins (safety check).")
                
                # calculate the adjusted desired throttle (above idle throttle, below governor throttle, scaled linearly)
                adj_throttle:float = throttle_idle + (throttle_range * input_throttle)

                # calculate errors - diff between the actual rates and the desired rates
                # "error" is calculated as setpoint (the goal) - actual
                error_rate_roll:float = (input_roll * max_rate_roll) - gyro_x
                error_rate_pitch:float = (input_pitch * max_rate_pitch) - gyro_y
                error_rate_yaw:float = (input_yaw * max_rate_yaw) - gyro_z

                # roll PID calc
                roll_p:float = error_rate_roll * pid_roll_kp
                roll_i:float = roll_last_integral + (error_rate_roll * pid_roll_ki * cycle_time_seconds)
                roll_i = max(min(roll_i, i_limit), -i_limit) # constrain within I-term limits
                roll_d:float = pid_roll_kd * (error_rate_roll - roll_last_error) / cycle_time_seconds
                pid_roll:float = roll_p + roll_i + roll_d

                # pitch PID calc
                pitch_p:float = error_rate_pitch * pid_pitch_kp
                pitch_i:float = pitch_last_integral + (error_rate_pitch * pid_pitch_ki * cycle_time_seconds)
                pitch_i = max(min(pitch_i, i_limit), -i_limit) # constrain within I-term limits
                pitch_d:float = pid_pitch_kd * (error_rate_pitch - pitch_last_error) / cycle_time_seconds
                pid_pitch = pitch_p + pitch_i + pitch_d

                # yaw PID calc
                yaw_p:float = error_rate_yaw * pid_yaw_kp
                yaw_i:float = yaw_last_integral + (error_rate_yaw * pid_yaw_ki * cycle_time_seconds)
                yaw_i = max(min(yaw_i, i_limit), -i_limit) # constrain within I-term limits
                yaw_d:float = pid_yaw_kd * (error_rate_yaw - yaw_last_error) / cycle_time_seconds
                pid_yaw = yaw_p + yaw_i + yaw_d

                # calculate throttle values
                t1:float = adj_throttle + pid_pitch + pid_roll - pid_yaw
                t2:float = adj_throttle + pid_pitch - pid_roll + pid_yaw
                t3:float = adj_throttle - pid_pitch + pid_roll + pid_yaw
                t4:float = adj_throttle - pid_pitch - pid_roll - pid_yaw

                # Adjust throttle according to input
                M1.duty_ns(calculate_duty_cycle(t1))
                M2.duty_ns(calculate_duty_cycle(t2))
                M3.duty_ns(calculate_duty_cycle(t3))
                M4.duty_ns(calculate_duty_cycle(t4))

                # Save state values for next loop
                roll_last_error = error_rate_roll
                pitch_last_error = error_rate_pitch
                yaw_last_error = error_rate_yaw
                roll_last_integral = roll_i
                pitch_last_integral = pitch_i
                yaw_last_integral = yaw_i

                # set last mode
                last_mode = True # True = flight mode (props spinning, pid active, motors receiving power commands, etc)

            else: # the input from channel 5 is unexpected
                print("Channel 5 input '" + str(rc_data[5]) + "' not valid. Is the transmitter turned on and connected?")

            # mark end time
            loop_end_us:int = time.ticks_us()

            # wait to make the hz correct
            elapsed_us:int = loop_end_us - loop_begin_us
            if elapsed_us < cycle_time_us:
                time.sleep_us(cycle_time_us - elapsed_us)
        

    except Exception as e: # something went wrong. Flash the LED so the pilot sees it

        # before we do anything, turn the motors OFF
        duty_0_percent:int = calculate_duty_cycle(0.0)
        M1.duty_ns(duty_0_percent)
        M2.duty_ns(duty_0_percent)
        M3.duty_ns(duty_0_percent)
        M4.duty_ns(duty_0_percent)

        # deinit
        M1.deinit()
        M2.deinit()
        M3.deinit()
        M4.deinit()
        
        FATAL_ERROR(str(e))















# UTILITY FUNCTIONS BELOW (Anything that is used by the flight controller loop should go here, not in a separate module or class (to save on processing time)

def calculate_duty_cycle(throttle:float, dead_zone:float = 0.03) -> int:
    """Determines the appropriate PWM duty cycle, in nanoseconds, to use for an ESC controlling a BLDC motor"""

    ### SETTINGS (that aren't parameters) ###
    duty_ceiling:int = 2000000 # the maximum duty cycle (max throttle, 100%) is 2 ms, or 10% duty (0.10)
    duty_floor:int = 1000000 # the minimum duty cycle (min throttle, 0%) is 1 ms, or 5% duty (0.05). HOWEVER, I've observed some "twitching" at exactly 5% duty cycle. It is off, but occasionally clips above, triggering the motor temporarily. To prevent this, i'm bringing the minimum down to slightly below 5%
    ################

    # calcualte the filtered percentage (consider dead zone)
    range:float = 1.0 - dead_zone - dead_zone
    percentage:float = min(max((throttle - dead_zone) / range, 0.0), 1.0)
    
    dutyns:int = duty_floor + ((duty_ceiling - duty_floor) * percentage)

    # clamp within the range
    dutyns = max(duty_floor, min(dutyns, duty_ceiling))

    return int(dutyns)

def normalize(value:float, original_min:float, original_max:float, new_min:float, new_max:float) -> float:
    """Normalizes (scales) a value to within a specific range."""
    return new_min + ((new_max - new_min) * ((value - original_min) / (original_max - original_min)))

def translate_pair(high:int, low:int) -> int:
        """Converts a byte pair to a usable value. Borrowed from https://github.com/m-rtijn/mpu6050/blob/0626053a5e1182f4951b78b8326691a9223a5f7d/mpu6050/mpu6050.py#L76C39-L76C39."""
        value = (high << 8) + low
        if value >= 0x8000:
            value = -((65535 - value) + 1)
        return value   

def FATAL_ERROR(msg:str) -> None:
    em:str = "Fatal error @ " + str(time.ticks_ms()) + " ms: " + msg
    print(em)
    toolkit.log(em)
    led = machine.Pin(25, machine.Pin.OUT)
    while True:
            led.off()
            time.sleep(1.0)
            led.on()
            time.sleep(1.0)








########### RUN THE SCOUT FLIGHT CONTROLLER PROGRAM ###########
run()