Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • danc/MicroCART
  • snawerdt/MicroCART_17-18
  • bbartels/MicroCART_17-18
  • jonahu/MicroCART
4 results
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 2600 additions and 0 deletions
quad/sw/imu_logger/src/util.h 0 → 100644
View file @ f6378157
/*
* util.h
*
* Created on: Oct 11, 2014
* Author: ucart
*/
#ifndef _UTIL_H
#define _UTIL_H
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <xgpiops.h>
#include "PID.h"
#include "log_data.h"
#include <sleep.h>
#include "controllers.h"
#include "xparameters.h"
#include "uart.h"
#define clock_rate 100000000
#define frequency 450
#define period_width clock_rate/frequency
#define pulse_throttle_low clock_rate / 1000
#define pulse_throttle_high clock_rate / 500
#define MOTOR_0_PERCENT 115000
#define XPAR_BTNS_BASEADDR 0x41200000
/**
* Various Addresses of custom IP components
*/
#define PWM_0_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_0_BASEADDR
#define PWM_1_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_1_BASEADDR
#define PWM_2_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_2_BASEADDR
#define PWM_3_ADDR XPAR_PWM_SIGNAL_OUT_WKILLSWITCH_3_BASEADDR
#define PWM_REC_0_ADDR XPAR_PWM_RECORDER_0_BASEADDR
#define PWM_REC_1_ADDR XPAR_PWM_RECORDER_1_BASEADDR
#define PWM_REC_2_ADDR XPAR_PWM_RECORDER_2_BASEADDR
#define PWM_REC_3_ADDR XPAR_PWM_RECORDER_3_BASEADDR
#define PWM_REC_4_ADDR XPAR_PWM_RECORDER_4_BASEADDR
#define PWM_REC_5_ADDR XPAR_PWM_RECORDER_5_BASEADDR
/**
* Register offsets within the custom IP
*/
#define PWM_PERIOD 0
#define PWM_PULSE 4
void pwm_init();
void pwm_write_all(int pulseWidth);
void pwm_write_channel(int pulseWidth, int channel);
int read_rec(int channel);
void read_rec_all(int* mixer);
void read_bluetooth_all(int* mixer);
void b_drive_pulse();
void sine_example();
void print_mixer(int* mixer);
int read_kill(int gear);
int read_flap(int flap);
void pwm_kill();
void printLogging();
void bluetoothTunePID(char instr, gam_t* gam, PID_t* CFpid, PID_t* Gpid);
void msleep(int msecs);
/**
* @brief
* Flashes the MIO7 LED how_many_times times and with ms_between_flashes between the flashes.
*
* @param how_many_times
* times the LED should be flashed
*
* @param ms_between_flashes
* time between flashes in milliseconds
*
*/
void flash_MIO_7_led(int how_many_times, int ms_between_flashes);
/**
* @brief
* Turns off MIO7 LED.
*
*/
void MIO7_led_off();
/**
* @brief
* Turns on MIO7 LED.
*
*/
void MIO7_led_on();
#endif //_UTIL_H
quad/sw/modular_quad_pid/.cproject 0 → 100644
View file @ f6378157
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="xilinx.gnu.arm.exe.debug.980189137">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.debug.980189137" moduleId="org.eclipse.cdt.core.settings" name="Debug">
<externalSettings/>
<extensions>
<extension id="com.xilinx.sdk.managedbuilder.XELF.arm" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.debug,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.debug.980189137" name="Debug" parent="xilinx.gnu.arm.exe.debug">
<folderInfo id="xilinx.gnu.arm.exe.debug.980189137." name="/" resourcePath="">
<toolChain id="xilinx.gnu.arm.exe.debug.toolchain.1195127676" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.debug.toolchain">
<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.debug.2072264921" isAbstract="false" name="Debug Platform" superClass="xilinx.arm.target.gnu.base.debug"/>
<builder buildPath="${workspace_loc:/modular_quad_pid}/Debug" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.debug.2124876787" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="GNU make" superClass="xilinx.gnu.arm.toolchain.builder.debug"/>
<tool id="xilinx.gnu.arm.c.toolchain.assembler.debug.192056667" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.debug">
<inputType id="xilinx.gnu.assembler.input.532832448" superClass="xilinx.gnu.assembler.input"/>
</tool>
<tool id="xilinx.gnu.arm.c.toolchain.compiler.debug.177835003" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.debug">
<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.1900496019" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" value="gnu.c.optimization.level.none" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.option.debugging.level.1207856754" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.inferred.swplatform.includes.2123463819" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
</option>
<option id="xilinx.gnu.compiler.symbols.defined.1696008720" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined"/>
<inputType id="xilinx.gnu.arm.c.compiler.input.909725989" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
</tool>
<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.debug.1470236349" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.debug">
<option defaultValue="gnu.c.optimization.level.none" id="xilinx.gnu.compiler.option.optimization.level.9972228" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.option.debugging.level.595503730" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.max" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.inferred.swplatform.includes.53367111" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
</option>
</tool>
<tool id="xilinx.gnu.arm.toolchain.archiver.585254344" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
<tool id="xilinx.gnu.arm.c.toolchain.linker.debug.360488201" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.debug">
<option id="xilinx.gnu.linker.inferred.swplatform.lpath.47881921" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
</option>
<option id="xilinx.gnu.linker.inferred.swplatform.flags.349145084" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
</option>
<option id="xilinx.gnu.c.linker.option.lscript.972374298" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
<option id="xilinx.gnu.c.link.option.libs.1025826490" name="Libraries (-l)" superClass="xilinx.gnu.c.link.option.libs" valueType="libs">
<listOptionValue builtIn="false" value="m"/>
</option>
<inputType id="xilinx.gnu.linker.input.777404607" superClass="xilinx.gnu.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
<inputType id="xilinx.gnu.linker.input.lscript.634888687" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
</tool>
<tool id="xilinx.gnu.arm.cxx.toolchain.linker.debug.1921379343" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.debug">
<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1819794269" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
</option>
<option id="xilinx.gnu.linker.inferred.swplatform.flags.1650054208" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
</option>
<option id="xilinx.gnu.c.linker.option.lscript.1506564015" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
</tool>
<tool id="xilinx.gnu.arm.size.debug.923967150" name="ARM Print Size" superClass="xilinx.gnu.arm.size.debug"/>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
<cconfiguration id="xilinx.gnu.arm.exe.release.255973624">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="xilinx.gnu.arm.exe.release.255973624" moduleId="org.eclipse.cdt.core.settings" name="Release">
<externalSettings/>
<extensions>
<extension id="com.xilinx.sdk.managedbuilder.XELF.arm" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactExtension="elf" artifactName="${ProjName}" buildArtefactType="org.eclipse.cdt.build.core.buildArtefactType.exe" buildProperties="org.eclipse.cdt.build.core.buildType=org.eclipse.cdt.build.core.buildType.release,org.eclipse.cdt.build.core.buildArtefactType=org.eclipse.cdt.build.core.buildArtefactType.exe" cleanCommand="rm -rf" description="" id="xilinx.gnu.arm.exe.release.255973624" name="Release" parent="xilinx.gnu.arm.exe.release">
<folderInfo id="xilinx.gnu.arm.exe.release.255973624." name="/" resourcePath="">
<toolChain id="xilinx.gnu.arm.exe.release.toolchain.1375808537" name="Xilinx ARM GNU Toolchain" superClass="xilinx.gnu.arm.exe.release.toolchain">
<targetPlatform binaryParser="com.xilinx.sdk.managedbuilder.XELF.arm" id="xilinx.arm.target.gnu.base.release.230721338" isAbstract="false" name="Release Platform" superClass="xilinx.arm.target.gnu.base.release"/>
<builder buildPath="${workspace_loc:/modular_quad_pid}/Release" enableAutoBuild="true" id="xilinx.gnu.arm.toolchain.builder.release.122813927" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="GNU make" superClass="xilinx.gnu.arm.toolchain.builder.release"/>
<tool id="xilinx.gnu.arm.c.toolchain.assembler.release.1899854338" name="ARM gcc assembler" superClass="xilinx.gnu.arm.c.toolchain.assembler.release">
<inputType id="xilinx.gnu.assembler.input.756511915" superClass="xilinx.gnu.assembler.input"/>
</tool>
<tool id="xilinx.gnu.arm.c.toolchain.compiler.release.85270120" name="ARM gcc compiler" superClass="xilinx.gnu.arm.c.toolchain.compiler.release">
<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.515686013" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.option.debugging.level.1121150517" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.inferred.swplatform.includes.687694973" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
</option>
<option id="xilinx.gnu.compiler.symbols.defined.1562495938" name="Defined symbols (-D)" superClass="xilinx.gnu.compiler.symbols.defined" valueType="definedSymbols">
<listOptionValue builtIn="false" value="NDEBUG=1"/>
</option>
<inputType id="xilinx.gnu.arm.c.compiler.input.846429887" name="C source files" superClass="xilinx.gnu.arm.c.compiler.input"/>
</tool>
<tool id="xilinx.gnu.arm.cxx.toolchain.compiler.release.1846278293" name="ARM g++ compiler" superClass="xilinx.gnu.arm.cxx.toolchain.compiler.release">
<option defaultValue="gnu.c.optimization.level.more" id="xilinx.gnu.compiler.option.optimization.level.1613253262" name="Optimization Level" superClass="xilinx.gnu.compiler.option.optimization.level" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.option.debugging.level.1485305325" name="Debug Level" superClass="xilinx.gnu.compiler.option.debugging.level" value="gnu.c.debugging.level.none" valueType="enumerated"/>
<option id="xilinx.gnu.compiler.inferred.swplatform.includes.2144106422" name="Software Platform Include Path" superClass="xilinx.gnu.compiler.inferred.swplatform.includes" valueType="includePath">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/include"/>
</option>
</tool>
<tool id="xilinx.gnu.arm.toolchain.archiver.86822110" name="ARM archiver" superClass="xilinx.gnu.arm.toolchain.archiver"/>
<tool id="xilinx.gnu.arm.c.toolchain.linker.release.264708896" name="ARM gcc linker" superClass="xilinx.gnu.arm.c.toolchain.linker.release">
<option id="xilinx.gnu.linker.inferred.swplatform.lpath.444912795" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
</option>
<option id="xilinx.gnu.linker.inferred.swplatform.flags.1169214283" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
</option>
<option id="xilinx.gnu.c.linker.option.lscript.1266246445" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
<option id="xilinx.gnu.c.link.option.libs.962400757" name="Libraries (-l)" superClass="xilinx.gnu.c.link.option.libs" valueType="libs">
<listOptionValue builtIn="false" value="m"/>
</option>
<inputType id="xilinx.gnu.linker.input.1933167678" superClass="xilinx.gnu.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
<inputType id="xilinx.gnu.linker.input.lscript.574206172" name="Linker Script" superClass="xilinx.gnu.linker.input.lscript"/>
</tool>
<tool id="xilinx.gnu.arm.cxx.toolchain.linker.release.1512126497" name="ARM g++ linker" superClass="xilinx.gnu.arm.cxx.toolchain.linker.release">
<option id="xilinx.gnu.linker.inferred.swplatform.lpath.1491656562" name="Software Platform Library Path" superClass="xilinx.gnu.linker.inferred.swplatform.lpath" valueType="libPaths">
<listOptionValue builtIn="false" value="../../system_bsp/ps7_cortexa9_0/lib"/>
</option>
<option id="xilinx.gnu.linker.inferred.swplatform.flags.1353247076" name="Software Platform Inferred Flags" superClass="xilinx.gnu.linker.inferred.swplatform.flags" valueType="libs">
<listOptionValue builtIn="false" value="-Wl,--start-group,-lxil,-lgcc,-lc,--end-group"/>
</option>
<option id="xilinx.gnu.c.linker.option.lscript.486002186" name="Linker Script" superClass="xilinx.gnu.c.linker.option.lscript" value="../src/lscript.ld" valueType="string"/>
</tool>
<tool id="xilinx.gnu.arm.size.release.362029751" name="ARM Print Size" superClass="xilinx.gnu.arm.size.release"/>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="modular_quad_pid.xilinx.gnu.arm.exe.832182557" name="Xilinx ARM Executable" projectType="xilinx.gnu.arm.exe"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId=""/>
<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.980189137;xilinx.gnu.arm.exe.debug.980189137.">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.255973624;xilinx.gnu.arm.exe.release.255973624.;xilinx.gnu.arm.c.toolchain.compiler.release.85270120;xilinx.gnu.arm.c.compiler.input.846429887">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.release.255973624;xilinx.gnu.arm.exe.release.255973624.">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
</scannerConfigBuildInfo>
<scannerConfigBuildInfo instanceId="xilinx.gnu.arm.exe.debug.980189137;xilinx.gnu.arm.exe.debug.980189137.;xilinx.gnu.arm.c.toolchain.compiler.debug.177835003;xilinx.gnu.arm.c.compiler.input.909725989">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="com.xilinx.managedbuilder.ui.ARMGCCManagedMakePerProjectProfileC"/>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Release">
<resource resourceType="PROJECT" workspacePath="/modular_quad_pid"/>
</configuration>
<configuration configurationName="Debug">
<resource resourceType="PROJECT" workspacePath="/modular_quad_pid"/>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cproject>
quad/sw/modular_quad_pid/.gitignore 0 → 100644
View file @ f6378157
Debug/
bootimage/
quad/sw/modular_quad_pid/.project 0 → 100644
View file @ f6378157
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>modular_quad_pid</name>
<comment></comment>
<projects>
<project>system_bsp_new</project>
<project>system_bsp</project>
</projects>
<buildSpec>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.genmakebuilder</name>
<arguments>
</arguments>
</buildCommand>
<buildCommand>
<name>org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder</name>
<triggers>full,incremental,</triggers>
<arguments>
</arguments>
</buildCommand>
</buildSpec>
<natures>
<nature>org.eclipse.cdt.core.cnature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>
<nature>org.eclipse.cdt.managedbuilder.core.ScannerConfigNature</nature>
</natures>
</projectDescription>
quad/sw/modular_quad_pid/src/Copy of original lscript.ld 0 → 100644
View file @ f6378157
/*******************************************************************/
/* */
/* This file is automatically generated by linker script generator.*/
/* */
/* Version: Xilinx EDK 14.7 EDK_P.20131013 */
/* */
/* Copyright (c) 2010 Xilinx, Inc. All rights reserved. */
/* */
/* Description : Cortex-A9 Linker Script */
/* */
/*******************************************************************/
_STACK_SIZE = DEFINED(_STACK_SIZE) ? _STACK_SIZE : 0x2000;
_HEAP_SIZE = DEFINED(_HEAP_SIZE) ? _HEAP_SIZE : 0x2000;
_ABORT_STACK_SIZE = DEFINED(_ABORT_STACK_SIZE) ? _ABORT_STACK_SIZE : 1024;
_SUPERVISOR_STACK_SIZE = DEFINED(_SUPERVISOR_STACK_SIZE) ? _SUPERVISOR_STACK_SIZE : 2048;
_FIQ_STACK_SIZE = DEFINED(_FIQ_STACK_SIZE) ? _FIQ_STACK_SIZE : 1024;
_UNDEF_STACK_SIZE = DEFINED(_UNDEF_STACK_SIZE) ? _UNDEF_STACK_SIZE : 1024;
/* Define Memories in the system */
MEMORY
{
ps7_ddr_0_S_AXI_BASEADDR : ORIGIN = 0x00100000, LENGTH = 0x1FF00000
ps7_ram_0_S_AXI_BASEADDR : ORIGIN = 0x00000000, LENGTH = 0x00030000
ps7_ram_1_S_AXI_BASEADDR : ORIGIN = 0xFFFF0000, LENGTH = 0x0000FE00
}
/* Specify the default entry point to the program */
ENTRY(_vector_table)
/* Define the sections, and where they are mapped in memory */
SECTIONS
{
.text : {
*(.vectors)
*(.boot)
*(.text)
*(.text.*)
*(.gnu.linkonce.t.*)
*(.plt)
*(.gnu_warning)
*(.gcc_execpt_table)
*(.glue_7)
*(.glue_7t)
*(.vfp11_veneer)
*(.ARM.extab)
*(.gnu.linkonce.armextab.*)
} > ps7_ddr_0_S_AXI_BASEADDR
.init : {
KEEP (*(.init))
} > ps7_ddr_0_S_AXI_BASEADDR
.fini : {
KEEP (*(.fini))
} > ps7_ddr_0_S_AXI_BASEADDR
.rodata : {
__rodata_start = .;
*(.rodata)
*(.rodata.*)
*(.gnu.linkonce.r.*)
__rodata_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.rodata1 : {
__rodata1_start = .;
*(.rodata1)
*(.rodata1.*)
__rodata1_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.sdata2 : {
__sdata2_start = .;
*(.sdata2)
*(.sdata2.*)
*(.gnu.linkonce.s2.*)
__sdata2_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.sbss2 : {
__sbss2_start = .;
*(.sbss2)
*(.sbss2.*)
*(.gnu.linkonce.sb2.*)
__sbss2_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.data : {
__data_start = .;
*(.data)
*(.data.*)
*(.gnu.linkonce.d.*)
*(.jcr)
*(.got)
*(.got.plt)
__data_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.data1 : {
__data1_start = .;
*(.data1)
*(.data1.*)
__data1_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.got : {
*(.got)
} > ps7_ddr_0_S_AXI_BASEADDR
.ctors : {
__CTOR_LIST__ = .;
___CTORS_LIST___ = .;
KEEP (*crtbegin.o(.ctors))
KEEP (*(EXCLUDE_FILE(*crtend.o) .ctors))
KEEP (*(SORT(.ctors.*)))
KEEP (*(.ctors))
__CTOR_END__ = .;
___CTORS_END___ = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.dtors : {
__DTOR_LIST__ = .;
___DTORS_LIST___ = .;
KEEP (*crtbegin.o(.dtors))
KEEP (*(EXCLUDE_FILE(*crtend.o) .dtors))
KEEP (*(SORT(.dtors.*)))
KEEP (*(.dtors))
__DTOR_END__ = .;
___DTORS_END___ = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.fixup : {
__fixup_start = .;
*(.fixup)
__fixup_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.eh_frame : {
*(.eh_frame)
} > ps7_ddr_0_S_AXI_BASEADDR
.eh_framehdr : {
__eh_framehdr_start = .;
*(.eh_framehdr)
__eh_framehdr_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.gcc_except_table : {
*(.gcc_except_table)
} > ps7_ddr_0_S_AXI_BASEADDR
.mmu_tbl (ALIGN(16384)) : {
__mmu_tbl_start = .;
*(.mmu_tbl)
__mmu_tbl_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.ARM.exidx : {
__exidx_start = .;
*(.ARM.exidx*)
*(.gnu.linkonce.armexidix.*.*)
__exidx_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.preinit_array : {
__preinit_array_start = .;
KEEP (*(SORT(.preinit_array.*)))
KEEP (*(.preinit_array))
__preinit_array_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.init_array : {
__init_array_start = .;
KEEP (*(SORT(.init_array.*)))
KEEP (*(.init_array))
__init_array_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.fini_array : {
__fini_array_start = .;
KEEP (*(SORT(.fini_array.*)))
KEEP (*(.fini_array))
__fini_array_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.ARM.attributes : {
__ARM.attributes_start = .;
*(.ARM.attributes)
__ARM.attributes_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.sdata : {
__sdata_start = .;
*(.sdata)
*(.sdata.*)
*(.gnu.linkonce.s.*)
__sdata_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.sbss (NOLOAD) : {
__sbss_start = .;
*(.sbss)
*(.sbss.*)
*(.gnu.linkonce.sb.*)
__sbss_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.tdata : {
__tdata_start = .;
*(.tdata)
*(.tdata.*)
*(.gnu.linkonce.td.*)
__tdata_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.tbss : {
__tbss_start = .;
*(.tbss)
*(.tbss.*)
*(.gnu.linkonce.tb.*)
__tbss_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.bss (NOLOAD) : {
__bss_start = .;
*(.bss)
*(.bss.*)
*(.gnu.linkonce.b.*)
*(COMMON)
__bss_end = .;
} > ps7_ddr_0_S_AXI_BASEADDR
_SDA_BASE_ = __sdata_start + ((__sbss_end - __sdata_start) / 2 );
_SDA2_BASE_ = __sdata2_start + ((__sbss2_end - __sdata2_start) / 2 );
/* Generate Stack and Heap definitions */
.heap (NOLOAD) : {
. = ALIGN(16);
_heap = .;
HeapBase = .;
_heap_start = .;
. += _HEAP_SIZE;
_heap_end = .;
HeapLimit = .;
} > ps7_ddr_0_S_AXI_BASEADDR
.stack (NOLOAD) : {
. = ALIGN(16);
_stack_end = .;
. += _STACK_SIZE;
_stack = .;
__stack = _stack;
. = ALIGN(16);
_irq_stack_end = .;
. += _STACK_SIZE;
__irq_stack = .;
_supervisor_stack_end = .;
. += _SUPERVISOR_STACK_SIZE;
. = ALIGN(16);
__supervisor_stack = .;
_abort_stack_end = .;
. += _ABORT_STACK_SIZE;
. = ALIGN(16);
__abort_stack = .;
_fiq_stack_end = .;
. += _FIQ_STACK_SIZE;
. = ALIGN(16);
__fiq_stack = .;
_undef_stack_end = .;
. += _UNDEF_STACK_SIZE;
. = ALIGN(16);
__undef_stack = .;
} > ps7_ddr_0_S_AXI_BASEADDR
_end = .;
}
quad/sw/modular_quad_pid/src/PID.c 0 → 100644
View file @ f6378157
/*
* PID.c
*
* Created on: Nov 10, 2014
* Author: ucart
*/
#include "PID.h"
#include <math.h>
#include <float.h>
// The generic PID diagram. This function takes in pid parameters (PID_t * pid) and calculates the output "pid_correction"
// part based on those parameters.
//
// + --- error ------------------ P + --- ----------------------------
// setpoint ---> / sum \ --------->| Kp * error |--------------->/ sum \ -------->| output: "pid_correction" |
// \ / | ------------------ \ / ----------------------------
// --- | --- ||
// - ^ | + ^ ^ + ||
// | | ------------------------------- | | ------- \/------------
// | |----->| Ki * accumulated error * dt |----+ | | |
// | | ------------------------------- I | | SYSTEM |
// | | | | |
// | | | --------||------------
// | | | ||
// | | ---------------------------------- | ||
// | |----->| Kd * (error - last error) / dt |----+ ||
// | ---------------------------------- D ||
// | ||
// | -----------\/-----------
// |____________________________________________________________| Sensor measurements: |
// | "current point" |
// ------------------------
//
PID_values pid_computation(PID_t *pid) {
float P = 0.0, I = 0.0, D = 0.0;
// calculate the current error
float error = pid->setpoint - pid->current_point;
// Accumulate the error (if Ki is less than epsilon, rougly 0,
// then reset the accumulated error for safety)
if (fabs(pid->Ki) <= FLT_EPSILON) {
pid->acc_error = 0;
} else {
pid->acc_error += error;
}
float change_in_error = error - pid->prev_error;
// Compute each term's contribution
P = pid->Kp * error;
I = pid->Ki * pid->acc_error * pid->dt;
D = pid->Kd * (change_in_error / pid->dt);
PID_values ret = {P, I, D, error, change_in_error, P + I + D};
pid->prev_error = error; // Store the current error into the PID_t
pid->pid_correction = P + I + D; // Store the computed correction
return ret;
}
quad/sw/modular_quad_pid/src/PID.h 0 → 100644
View file @ f6378157
/*
* PID.h
*
* Created on: Nov 10, 2014
* Author: ucart
*/
#ifndef PID_H_
#define PID_H_
#include "type_def.h"
// Yaw constants
// when using units of degrees
//#define YAW_ANGULAR_VELOCITY_KP 40.0f
//#define YAW_ANGULAR_VELOCITY_KI 0.0f
//#define YAW_ANGULAR_VELOCITY_KD 0.0f
//#define YAW_ANGLE_KP 2.6f
//#define YAW_ANGLE_KI 0.0f
//#define YAW_ANGLE_KD 0.0f
// when using units of radians
#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.0f
#define YAW_ANGULAR_VELOCITY_KI 0.0f
#define YAW_ANGULAR_VELOCITY_KD 0.0f
#define YAW_ANGLE_KP 2.6f
#define YAW_ANGLE_KI 0.0f
#define YAW_ANGLE_KD 0.0f
// Roll constants
//#define ROLL_ANGULAR_VELOCITY_KP 0.95f
//#define ROLL_ANGULAR_VELOCITY_KI 0.0f
//#define ROLL_ANGULAR_VELOCITY_KD 0.13f//0.4f//0.7f
//#define ROLL_ANGLE_KP 17.0f //9.0f
//#define ROLL_ANGLE_KI 0.0f
//#define ROLL_ANGLE_KD 0.3f // 0.2f
//#define YPOS_KP 0.0f
//#define YPOS_KI 0.0f
//#define YPOS_KD 0.0f
// when using units of radians
#define ROLL_ANGULAR_VELOCITY_KP 100.0f*46.0f//102.0f*46.0f//9384.0f//204.0f * 46.0f
#define ROLL_ANGULAR_VELOCITY_KI 0.0f
#define ROLL_ANGULAR_VELOCITY_KD 100.f*5.5f//102.0f*6.8f//1387.2//204.0f * 6.8f
#define ROLL_ANGLE_KP 15.0f
#define ROLL_ANGLE_KI 0.0f
#define ROLL_ANGLE_KD 0.2f
#define YPOS_KP 0.015f
#define YPOS_KI 0.005f
#define YPOS_KD 0.03f
//Pitch constants
// when using units of degrees
//#define PITCH_ANGULAR_VELOCITY_KP 0.95f
//#define PITCH_ANGULAR_VELOCITY_KI 0.0f
//#define PITCH_ANGULAR_VELOCITY_KD 0.13f//0.35f//0.7f
//#define PITCH_ANGLE_KP 17.0f // 7.2f
//#define PITCH_ANGLE_KI 0.0f
//#define PITCH_ANGLE_KD 0.3f //0.3f
//#define XPOS_KP 40.0f
//#define XPOS_KI 0.0f
//#define XPOS_KD 10.0f//0.015f
// when using units of radians
#define PITCH_ANGULAR_VELOCITY_KP 100.0f*46.0f//101.0f*46.0f//9292.0f//202.0f * 46.0f
#define PITCH_ANGULAR_VELOCITY_KI 0.0f
#define PITCH_ANGULAR_VELOCITY_KD 100.0f*5.5f//101.0f*6.8f//1373.6//202.0f * 6.8f
#define PITCH_ANGLE_KP 15.0f
#define PITCH_ANGLE_KI 0.0f
#define PITCH_ANGLE_KD 0.2f
#define XPOS_KP -0.015f
#define XPOS_KI -0.005f
#define XPOS_KD -0.03f
//Throttle constants
#define ALT_ZPOS_KP 9804.0f
#define ALT_ZPOS_KI 817.0f
#define ALT_ZPOS_KD 7353.0f
// Computes control error and correction
PID_values pid_computation(PID_t *pid);
#endif /* PID_H_ */
quad/sw/modular_quad_pid/src/README.txt 0 → 100644
View file @ f6378157
This application is the PID implementation of the modular control loop. This is the same implementation as the existing quad controller. The mixer in this application needs to be changed to be correct according to common implementation.
quad/sw/modular_quad_pid/src/actuator_command_processing.c 0 → 100644
View file @ f6378157
/*
* actuator_command_processing.c
*
* Created on: Feb 20, 2016
* Author: ucart
*/
#include "actuator_command_processing.h"
#include "sensor_processing.h"
int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct)
{
Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
// old_Aero_to_PWMS(actuator_command_struct->pwms, raw_actuator_struct->controller_corrected_motor_commands);
return 0;
}
/**
* Converts Aero 4 channel signals to PWM signals
* Aero channels are defined above
*/
void Aero_to_PWMS(int* PWMs, int* aero)
{
int motor0_bias = 0, motor1_bias = 0, motor2_bias = 0, motor3_bias = 0;
int pwm0 = 0, pwm1 = 0, pwm2 = 0, pwm3 = 0;
pwm0 = aero[THROTTLE] - aero[PITCH] - aero[ROLL] - aero[YAW] + motor0_bias;
pwm1 = aero[THROTTLE] + aero[PITCH] - aero[ROLL] + aero[YAW] + motor1_bias;
pwm2 = aero[THROTTLE] - aero[PITCH] + aero[ROLL] + aero[YAW] + motor2_bias;
pwm3 = aero[THROTTLE] + aero[PITCH] + aero[ROLL] - aero[YAW] + motor3_bias;
// printf("pwm0: %d\tpwm1: %d\tpwm2: %d\tpwm3: %d\n", pwm0, pwm1, pwm2, pwm3);
/**
* Boundary checks:
*
* #define min 100000
* #define max 200000
*/
if(pwm0 < min)
pwm0 = min;
else if(pwm0 > max)
pwm0 = max;
if(pwm1 < min)
pwm1 = min;
else if(pwm1 > max)
pwm1 = max;
if(pwm2 < min)
pwm2 = min;
else if(pwm2 > max)
pwm2 = max;
if(pwm3 < min)
pwm3 = min;
else if(pwm3 > max)
pwm3 = max;
PWMs[0] = pwm0;
PWMs[1] = pwm1;
PWMs[2] = pwm2;
PWMs[3] = pwm3;
// the array PWMs is then written directly to the PWM hardware registers
// the PWMs are in units of clock cycles, not percentage duty cycle
// use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
}
/**
* Converts Aero 4 channel signals to PWM signals
* Aero channels are defined above
*
* *deprecated
*/
void old_Aero_to_PWMS(int* PWMs, int* aero) {
int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;
// int motor0_bias = -5000, motor1_bias = 0, motor2_bias = -5000, motor3_bias = 0;
// Throttle, pitch, roll, yaw as a percentage of their max - Range 0.0 - 100.0
float throttle_100 = (aero[THROTTLE] - THROTTLE_MIN) / (THROTTLE_RANGE*1.0);
float pitch_100 = (aero[PITCH] - PITCH_MIN) / (PITCH_RANGE*1.0);
float roll_100 = (aero[ROLL] - ROLL_MIN) / (ROLL_RANGE*1.0);
float yaw_100 = (aero[YAW] - YAW_MIN) / (YAW_RANGE*1.0);
// This adds a +/- 300 ms range bias for the throttle
int throttle_base = BASE + (int) 60000 * (throttle_100 - .5);
// This adds a +/- 200 ms range bias for the pitch
int pitch_base = (int) 60000 * (pitch_100 - .5);
// This adds a +/- 200 ms range bias for the roll
int roll_base = (int) 60000 * (roll_100 - .5);
// This adds a +/- 75 ms range bias for the yaw
int yaw_base = (int) 15000 * (yaw_100 - .5);
int pwm0, pwm1, pwm2, pwm3;
pwm1 = throttle_base + pitch_base/2 - roll_base/2 + yaw_base + motor1_bias;
pwm3 = throttle_base + pitch_base/2 + roll_base/2 - yaw_base + motor3_bias;
pwm0 = throttle_base - pitch_base/2 - roll_base/2 - yaw_base + motor0_bias;
pwm2 = throttle_base - pitch_base/2 + roll_base/2 + yaw_base + motor2_bias;
/**
* Boundary checks:
*
* #define min 100000
* #define max 200000
*/
if(pwm0 < min)
pwm0 = min;
else if(pwm0 > max)
pwm0 = max;
if(pwm1 < min)
pwm1 = min;
else if(pwm1 > max)
pwm1 = max;
if(pwm2 < min)
pwm2 = min;
else if(pwm2 > max)
pwm2 = max;
if(pwm3 < min)
pwm3 = min;
else if(pwm3 > max)
pwm3 = max;
PWMs[0] = pwm0;
PWMs[1] = pwm1;
PWMs[2] = pwm2;
PWMs[3] = pwm3;
// the array PWMs is then written directly to the PWM hardware registers
// the PWMs are in units of clock cycles, not percentage duty cycle
// use pwm/222,222 to get the duty cycle. the freq is 450 Hz on a 100MHz clock
}
quad/sw/modular_quad_pid/src/actuator_command_processing.h 0 → 100644
View file @ f6378157
/*
* actuator_command_processing.h
*
* Created on: Feb 20, 2016
* Author: ucart
*/
#ifndef ACTUATOR_COMMAND_PROCESSING_H_
#define ACTUATOR_COMMAND_PROCESSING_H_
#include <stdio.h>
#include "log_data.h"
#include "control_algorithm.h"
/**
* @brief
* Processes the commands to the actuators.
*
* @param log_struct
* structure of the data to be logged
*
* @param raw_actuator_struct
* structure of the commmands outputted to go to the actuators
*
* @param actuator_command_struct
* structure of the commmands to go to the actuators
*
* @return
* error message
*
*/
int actuator_command_processing(log_t* log_struct, user_input_t * user_input_struct, raw_actuator_t* raw_actuator_struct, actuator_command_t* actuator_command_struct);
void old_Aero_to_PWMS(int* PWMs, int* aero);
void Aero_to_PWMS(int* PWMs, int* aero);
#endif /* ACTUATOR_COMMAND_PROCESSING_H_ */
quad/sw/modular_quad_pid/src/commands.c 0 → 100644
View file @ f6378157
#include "communication.h"
#include "commands.h"
#include "type_def.h"
#include "uart.h"
struct MessageType MessageTypes[MAX_TYPE] =
{
// DEBUG
{
// Message Type ID
0x00,
// Debug Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"debug",
// Type of the command data
stringType,
// Function pointer
&debug
},
// Echo subtype
{
// ID
0x01,
// Command text
"ack",
// Type of the command data
intType,
// Function pointer
&debug
},
// Log packet receiving
{
// ID
0x02,
// Command text
"packetlog",
// Type of the command data
stringType,
// Function pointer
&packetlog
},
// Get packet receiving logs
{
// ID
0x03,
// Command text
"getpacketlogs",
// Type of the command data
stringType,
// Function pointer
&getpacketlogs
}
}
},
// CALIBRATION
{
// Message Type ID
0x01,
// Calibration Subtypes
{
// yaw setpoint subtype
{
// ID
0x00,
// Command text
"setyaw",
// Type of the command data
floatType,
// Function pointer
&yawset
},
// yaw p constant subtype
{
// ID
0x01,
// Command text
"setyawp",
// Type of the command data
floatType,
// Function pointer
&yawp
},
// yaw d constant subtype
{
// ID
0x02,
// Command text
"setyawd",
// Type of the command data
floatType,
// Function pointer
&yawd
},
// roll setpoint subtype
{
// ID
0x03,
// Command text
"setroll",
// Type of the command data
floatType,
// Function pointer
&rollset
},
// roll p constant subtype
{
// ID
0x04,
// Command text
"setrollp",
// Type of the command data
floatType,
// Function pointer
&rollp
},
// roll d constant subtype
{
// ID
0x05,
// Command text
"setrolld",
// Type of the command data
floatType,
// Function pointer
&rolld
},
// pitch setpoint subtype
{
// ID
0x06,
// Command text
"setpitch",
// Type of the command data
floatType,
// Function pointer
&pitchset
},
// pitch p constant subtype
{
// ID
0x07,
// Command text
"setpitchp",
// Type of the command data
floatType,
// Function pointer
&pitchp
},
// pitch d constant subtype
{
// ID
0x08,
// Command text
"setpitchd",
// Type of the command data
floatType,
// Function pointer
&pitchd
},
// throttle setpoint subtype
{
// ID
0x09,
// Command text
"setthrottle",
// Type of the command data
floatType,
// Function pointer
&throttleset
},
// throttle p constant subtype
{
// ID
0x0A,
// Command text
"setthrottlep",
// Type of the command data
floatType,
// Function pointer
&throttlep
},
// throttle i constant subtype
{
// ID
0x0B,
// Command text
"setthrottlei",
// Type of the command data
floatType,
// Function pointer
&throttlei
},
// throttle d constant subtype
{
// ID
0x0C,
// Command text
"setthrottled",
// Type of the command data
floatType,
// Function pointer
&throttled
}
}
},
// REQUEST
{
// Message Type ID
0x02,
// Request Subtypes
{
// accelerometer subtype
{
// ID
0x00,
// Command text
"accelreq",
// Type of the command data
floatType,
// Function pointer
&accelreq
},
// gyroscope subtype
{
// ID
0x01,
// Command text
"gyroreq",
// Type of the command data
floatType,
// Function pointer
&gyroreq
},
// pitch angle subtype
{
// ID
0x02,
// Command text
"reqpitchangle",
// Type of the command data
floatType,
// Function pointer
&pitchanglereq
},
// roll angle subtype
{
// ID
0x03,
// Command text
"reqrollangle",
// Type of the command data
floatType,
// Function pointer
&rollanglereq
}
}
},
// RESPONSE
{
// Message Type ID
0x03,
// Response Subtypes
{
// accelerometer subtype
{
// ID
0x00,
// Command text
"respaccel",
// Type of the command data
floatType,
// Function pointer
&accelresp
},
// gyroscope subtype
{
// ID
0x01,
// Command text
"respgyro",
// Type of the command data
floatType,
// Function pointer
&gyroresp
},
// pitch angle subtype
{
// ID
0x02,
// Command text
"resppitchangle",
// Type of the command data
floatType,
// Function pointer
&pitchangleresp
},
// roll angle subtype
{
// ID
0x03,
// Command text
"resprollangle",
// Type of the command data
floatType,
// Function pointer
&rollangleresp
}
}
},
// UPDATE
{
// Message Type ID
0x04,
// Update Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"update",
// Type of the command data
stringType,
// Function pointer
&update
},
// BEGIN UPDATE subtype
{
// ID
0x01,
// Command text
"beginupdate",
// Type of the command data
stringType,
// Function pointer
&beginupdate
}
}
},
// LOG
{
// Message Type ID
0x05,
// Log Subtypes
{
// NONE subtype
{
// ID
0x00,
// Command text
"log",
// Type of the command data
stringType,
// Function pointer
&logdata
},
// Response subtype
{
// ID
0x01,
// Command text
"response",
// Type of the command data
stringType,
// Function pointer
&response
}
}
},
};
int debug(modular_structs_t *structs)
{
printf("function for debug\n");
return 0;
}
static int n_msg_received = 0;
static size_t total_payload_received = 0;
int packetlog(modular_structs_t* structs) {
n_msg_received += 1;
total_payload_received += uart_buff_data_length();
return 0;
}
int getpacketlogs(modular_structs_t* structs) {
char buf[255];
// Message logging number of messages received and size of payload received
int length = snprintf(buf, sizeof buf, "%d,%d", n_msg_received, total_payload_received);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
/* Handles receiving new location updates */
int update(modular_structs_t *structs)
{
//processUpdate(packet, &(structs->raw_sensor_struct.currentQuadPosition));
quadPosition_t* currentQuadPosition = &(structs->raw_sensor_struct.currentQuadPosition);
// Packet must come as [NEARPY], 4 bytes each
int packetId = uart_buff_data_get_u32(0);
// printf("Packet ID: %d\n", packetId);
float y_pos = uart_buff_data_get_float(4);
// printf("y_pos: %f\n", y_pos);
float x_pos = uart_buff_data_get_float(8);
// printf("x_pos: %f\n", x_pos);
float alt_pos = uart_buff_data_get_float(12);
// printf("alt_pos: %f\n", alt_pos);
float roll = uart_buff_data_get_float(16);
// printf("roll: %f\n", roll);
float pitch = uart_buff_data_get_float(20);
// printf("pitch: %f\n", pitch);
float yaw = uart_buff_data_get_float(24);
// printf("yaw: %f\n", yaw);
currentQuadPosition->packetId = packetId;
currentQuadPosition->y_pos = y_pos;
currentQuadPosition->x_pos = x_pos;
currentQuadPosition->alt_pos = alt_pos;
currentQuadPosition->roll = roll;
currentQuadPosition->pitch = pitch;
currentQuadPosition->yaw = yaw;
// Make location as fresh
structs->user_input_struct.locationFresh = 1;
return 0;
}
// This is called on the ground station to begin sending VRPN to the quad
int beginupdate(modular_structs_t *structs) {
structs->user_input_struct.receivedBeginUpdate = 1;
return 0;
}
int logdata(modular_structs_t *structs)
{
char *packet = uart_buff_get_packet();
printf("Logging: %s\n", packet);
free(packet);
return 0;
}
int response(modular_structs_t *structs)
{
char *packet = uart_buff_get_packet();
printf("This is the response: %s\n", packet);
free(packet);
return 0;
}
// ------------------------------------------------------------------
// Quad side implementation
// TODO: Erase memory leaks
int yawset(modular_structs_t *structs)
{
char buf[255] = {};
structs->setpoint_struct.desiredQuadPosition.yaw = uart_buff_data_get_float(0);
// Debug print statement
//printf("function for yawset: %f\n", structs->setpoint_struct.desiredQuadPosition.yaw);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.yaw);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int yawp(modular_structs_t *structs)
{
char buf[255] = {0};
structs->parameter_struct.yaw_angle_pid.Kp = uart_buff_data_get_float(0);
printf("function for yawp: %f\n", structs->parameter_struct.yaw_angle_pid.Kp);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kp to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kp);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int yawd(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.yaw_angle_pid.Kd = uart_buff_data_get_float(0);
printf("function for yawd: %f\n", structs->parameter_struct.yaw_angle_pid.Kd);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set yaw Kd to %.2f\r\n", structs->parameter_struct.yaw_angle_pid.Kd);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int rollset(modular_structs_t *structs)
{
char buf[255] = {};
structs->setpoint_struct.desiredQuadPosition.roll = uart_buff_data_get_float(0);
printf("function for rollset: %f\n", structs->setpoint_struct.desiredQuadPosition.roll);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set desired roll to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.roll);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int rollp(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.roll_angle_pid.Kp = uart_buff_data_get_float(0);
printf("function for rollp: %f\n", structs->parameter_struct.roll_angle_pid.Kp);
// Send a reply to the ground station
size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kp to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kp);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int rolld(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.roll_angle_pid.Kd = uart_buff_data_get_float(0);
printf("function for rolld: %f\n", structs->parameter_struct.roll_angle_pid.Kd);
// Send a reply to the ground station
size_t length = snprintf(buf, sizeof(buf), "Successfully set roll Kd to %.2f\r\n", structs->parameter_struct.roll_angle_pid.Kd);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int pitchset(modular_structs_t *structs)
{
char buf[255] = {};
structs->setpoint_struct.desiredQuadPosition.pitch = uart_buff_data_get_float(0);
printf("function for pitchset: %f\n", structs->setpoint_struct.desiredQuadPosition.pitch);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set desired pitch to %.2f radians\r\n", structs->setpoint_struct.desiredQuadPosition.pitch);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int pitchp(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.pitch_angle_pid.Kp = uart_buff_data_get_float(0);
printf("function for pitchp: %f\n", structs->parameter_struct.pitch_angle_pid.Kp);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set pitch Kp to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kp);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int pitchd(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.pitch_angle_pid.Kd = uart_buff_data_get_float(0);
printf("function for pitchd: %f\n", structs->parameter_struct.pitch_angle_pid.Kd);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set desired yaw to %.2f\r\n", structs->parameter_struct.pitch_angle_pid.Kd);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
// ------------------------------------------------------------
// These should be renamed to altitude!
int throttleset(modular_structs_t *structs)
{
char buf[255] = {};
structs->setpoint_struct.desiredQuadPosition.alt_pos = uart_buff_data_get_float(0);
printf("function for throttleset: %f\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set desired altitude to %.2f meters\r\n", structs->setpoint_struct.desiredQuadPosition.alt_pos);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int throttlep(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.alt_pid.Kp = uart_buff_data_get_float(0);
printf("function for throttlep: %f\n", structs->parameter_struct.alt_pid.Kp);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set alt Kp to %.2f\r\n", structs->parameter_struct.alt_pid.Kp);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int throttlei(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.alt_pid.Ki = uart_buff_data_get_float(0);
printf("function for throttlei: %f\n", structs->parameter_struct.alt_pid.Ki);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set alt Ki to %.2f\r\n", structs->parameter_struct.alt_pid.Ki);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
int throttled(modular_structs_t *structs)
{
char buf[255] = {};
structs->parameter_struct.alt_pid.Kd = uart_buff_data_get_float(0);
printf("function for throttled: %f\n", structs->parameter_struct.alt_pid.Kd);
// Send a reply to the ground station
int length = snprintf(buf, sizeof(buf), "Successfully set alt Kd to %.2f\r\n", structs->parameter_struct.alt_pid.Kd);
send_data(MessageTypes[5].ID, MessageTypes[5].subtypes[1].ID, 0, buf, length >= sizeof(buf) ? 255 : length + 1);
return 0;
}
// These should be renamed to altitude!
// ------------------------------------------------------------
int accelreq(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int gyroresp(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int pitchangleresp(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int rollangleresp(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int gyroreq(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int pitchanglereq(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int rollanglereq(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
int accelresp(modular_structs_t *structs)
{
printf("function for accelreq\n");
return 0;
}
quad/sw/modular_quad_pid/src/commands.h 0 → 100644
View file @ f6378157
#ifndef _COMMANDS_H
#define _COMMANDS_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "type_def.h"
#include "packet_processing.h"
// ----------------------
// Helper stuff
#define MAX_TYPE 6
#define MAX_SUBTYPE 100
enum Message{
BEGIN_CHAR = 0xBE,
END_CHAR = 0xED
};
// This should also have double to avoid confusion with float values.
enum DataType
{
floatType,
intType,
stringType
};
// MESSAGE SUBTYPES
struct MessageSubtype{
char ID;
char cmdText[100];
char cmdDataType;
int (*functionPtr)(modular_structs_t *structs);
};
// MESSAGE TYPES
struct MessageType{
char ID;
struct MessageSubtype subtypes[MAX_SUBTYPE];
};
int debug(modular_structs_t *structs);
int packetlog(modular_structs_t* structs);
int getpacketlogs(modular_structs_t* structs);
int update(modular_structs_t *structs);
int beginupdate(modular_structs_t *structs);
int logdata(modular_structs_t *structs);
int response(modular_structs_t *structs);
int yawset(modular_structs_t *structs);
int yawp(modular_structs_t *structs);
int yawd(modular_structs_t *structs);
int rollset(modular_structs_t *structs);
int rollp(modular_structs_t *structs);
int rolld(modular_structs_t *structs);
int pitchset(modular_structs_t *structs);
int pitchp(modular_structs_t *structs);
int pitchd(modular_structs_t *structs);
int throttleset(modular_structs_t *structs);
int throttlep(modular_structs_t *structs);
int throttlei(modular_structs_t *structs);
int throttled(modular_structs_t *structs);
int accelreq(modular_structs_t *structs);
int gyroresp(modular_structs_t *structs);
int pitchangleresp(modular_structs_t *structs);
int rollangleresp(modular_structs_t *structs);
int gyroreq(modular_structs_t *structs);
int pitchanglereq(modular_structs_t *structs);
int rollanglereq(modular_structs_t *structs);
int accelresp(modular_structs_t *structs);
// TODO add in string to be read from the command line when sending a subtype of message
extern struct MessageType MessageTypes[MAX_TYPE];
#endif
quad/sw/modular_quad_pid/src/communication.c 0 → 100644
View file @ f6378157
#include "communication.h"
#include "uart_buff.h"
#define INTC XScuGic
#define COMM_UART_DEVICE_ID XPAR_PS7_UART_0_DEVICE_ID
#define COMM_INTC_DEVICE_ID XPAR_SCUGIC_SINGLE_DEVICE_ID
#define COMM_UART_INT_IRQ_ID XPAR_PS7_UART_0_INTR//XPAR_XUARTPS_0_INTR
#define BAUD_RATE 921600
// Maximum number of bytes to be received within our loop time,
// plus the maximum packet size that might be carried over from the last call,
// plus 128 for a little buffer
// (bit/s) * (seconds) / (10 bits / byte for UART)
#define MAX_PACKET_SIZE 256
#define UART_BUF_SIZE (((BAUD_RATE * (DESIRED_USEC_PER_LOOP / 1000) / 1000) / 10) + MAX_PACKET_SIZE + 128)
// Declaration of interrupt handler
void Handler(void *CallBackRef, u32 Event, unsigned int EventData);
// Pointer to the UART driver instance
static XUartPs* uartInstPtr;
int initUartComms() {
uartInstPtr = uart0_init(COMM_UART_DEVICE_ID, BAUD_RATE);
// Initialize UART0 (Bluetooth/WiFi)
if(!uartInstPtr) {
return -1;
}
uart0_clearFIFOs();
if (uart0_int_init(COMM_UART_INT_IRQ_ID, (Xil_ExceptionHandler) uart_interrupt_handler) != XST_SUCCESS) {
return -1;
}
/*
* Setup the handlers for the UART that will be called from the
* interrupt context when data has been sent and received, specify
* a pointer to the UART driver instance as the callback reference
* so the handlers are able to access the instance data
*/
//XUartPs_SetHandler(uartInstPtr, (XUartPs_Handler)Handler, uartInstPtr);
u32 IntrMask = XUARTPS_IXR_RXFULL | XUARTPS_IXR_RXOVR | XUARTPS_IXR_TOUT;
XUartPs_SetInterruptMask(uartInstPtr, IntrMask);
/*
* Set the receiver timeout. If it is not set, and the last few bytes
* of data do not trigger the over-water or full interrupt, the bytes
* will not be received. By default it is disabled.
* Timeout duration = RecvTimeout x 4 x Bit Period. 0 disables the
* timeout function.
*
* The setting of 8 will timeout after 8 x 4 = 32 character times.
* Increase the time out value if baud rate is high, decrease it if
* baud rate is low.
*/
XUartPs_SetRecvTimeout(uartInstPtr, 8);
// Second argument is the number of bytes to trigger an interrupt at
XUartPs_SetFifoThreshold(uartInstPtr, 48);
return 0;
}
int process_packet(modular_structs_t *structs) {
metadata_t meta_data;
// parse metadata
meta_data.begin_char = uart_buff_get_u8(0);
meta_data.msg_type = uart_buff_get_u8(1);
meta_data.msg_subtype = uart_buff_get_u8(2);
meta_data.msg_id = uart_buff_get_u16(3);
meta_data.data_len = uart_buff_get_u16(5);
unsigned char packet_checksum = uart_buff_get_u8(7+meta_data.data_len);
//unsigned char* packet_data = packet + sizeof(metadata_t);
// Compute checksum
int i;
unsigned char calculated_checksum = 0;
for(i = 0; i < meta_data.data_len + 7; i++){
calculated_checksum ^= uart_buff_get_u8(i);
}
// Discard if checksum didn't match
if(packet_checksum != calculated_checksum) {
printf("Checksums did not match: 0x%02x\t0x%02x\n", packet_checksum, calculated_checksum);
return -1;
}
// Call appropriate function for packet
(* (MessageTypes[meta_data.msg_type].subtypes[meta_data.msg_subtype].functionPtr))(structs);
uart_buff_flush_packet();
return 0;
}
/*
* Temporarily disables interrupts and returns the interrupt state, for restoring them
*/
u32 disable_interrupts() {
u32 ImrRegister;
ImrRegister = XUartPs_ReadReg(uartInstPtr->Config.BaseAddress, XUARTPS_IMR_OFFSET);
XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IDR_OFFSET, XUARTPS_IXR_MASK);
return ImrRegister;
}
/*
* Restores the interrupt state, saved from disable_interrupts
*/
void restore_interrupts(u32 intr_state) {
XUartPs_WriteReg(uartInstPtr->Config.BaseAddress, XUARTPS_IER_OFFSET, intr_state);
}
void process_received(modular_structs_t *structs) {
// Parse as many packets as possible
while (uart_buff_packet_ready()) {
process_packet(structs);
}
}
void uart_interrupt_handler(XUartPs *InstancePtr) {
u32 IsrStatus;
/*
* Read the interrupt ID register to determine which
* interrupt is active
*/
IsrStatus = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
XUARTPS_IMR_OFFSET);
IsrStatus &= XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
XUARTPS_ISR_OFFSET);
/*
* Read the Channel Status Register to determine if there is any data in
* the RX FIFO
*/
u32 CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress,
XUARTPS_SR_OFFSET);
while (0 == (CsrRegister & XUARTPS_SR_RXEMPTY) && !uart_buff_full()) {
u8 byte = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_FIFO_OFFSET);
uart_buff_put_byte(byte);
CsrRegister = XUartPs_ReadReg(InstancePtr->Config.BaseAddress, XUARTPS_SR_OFFSET);
}
// Clear the interrupt status.
XUartPs_WriteReg(InstancePtr->Config.BaseAddress, XUARTPS_ISR_OFFSET,
IsrStatus);
}
int send_data(u16 type_id, u16 subtype_id, u16 msg_id, char* data, size_t size) {
//----------------------------------------------------------------------------------------------
// index|| 0 | 1 | 2 | 3 & 4 | 5 & 6 | 7+ | end |
//---------------------------------------------------------------------------------------------|
// msg param|| beg char | msg type | msg subtype | msg id | data len (bytes) | data | checksum |
//-------------------------------------------------------------------------------------------- |
// bytes|| 1 | 1 | 1 | 2 | 2 | var | 1 |
//----------------------------------------------------------------------------------------------
char formattedHeader[7];
// Begin Char:
formattedHeader[0] = BEGIN_CHAR;
// Msg type:
formattedHeader[1] = type_id;
// Msg subtype
formattedHeader[2] = subtype_id;
//Msg id 2 bytes
formattedHeader[3] = msg_id & 0x000000ff;
formattedHeader[4] = (msg_id >> 8) & 0x000000ff;
// Data length and data - bytes 5&6 for len, 7+ for data
formattedHeader[5] = size & 0x000000ff;
formattedHeader[6] = (size >> 8) & 0x000000ff;
// Compute checksum while sending
unsigned char packet_checksum = 0;
int i;
// TODO: Look into uart0_sendBytes and see if it would be better to use
// Send header
//uart0_sendBytes(formattedHeader, 7);
for(i = 0; i < 7; i++) {
packet_checksum ^= formattedHeader[i];
uart0_sendByte(formattedHeader[i]);
}
// Send data
for (i = 0; i < size; i++) {
packet_checksum ^= data[i];
uart0_sendByte(data[i]);
}
//uart0_sendBytes(data, size);
// Send checksum
uart0_sendByte(packet_checksum);
return 0;
}
quad/sw/modular_quad_pid/src/communication.h 0 → 100644
View file @ f6378157
#ifndef _COMMUNICATION_H
#define _COMMUNICATION_H
#include <xuartps.h>
#include "xparameters.h"
#include "xscugic.h"
#include "xil_exception.h"
#include "type_def.h"
#include "uart.h"
#include "mio7_led.h"
#include "timer.h"
#include "commands.h"
#include "uart_buff.h"
int initUartComms();
void process_received(modular_structs_t *structs);
void uart_interrupt_handler(XUartPs *InstancePtr);
int send_data(u16 type_id, u16 subtype_id, u16 msg_id, char* data, size_t size);
#endif
quad/sw/modular_quad_pid/src/control_algorithm.c 0 → 100644
View file @ f6378157
/*
* control_algorithm.c
*
* Created on: Feb 20, 2016
* Author: ucart
*/
// This implemented modular quadrotor software implements a PID control algorithm
#include "control_algorithm.h"
#include "communication.h"
#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
int control_algorithm_init(parameter_t * parameter_struct)
{
// HUMAN Piloted (RC) PID DEFINITIONS //////
// RC PIDs for roll (2 loops: angle --> angular velocity)
parameter_struct->roll_angle_pid.dt = 0.005; parameter_struct->roll_ang_vel_pid.dt = 0.005; // 5 ms calculation period
// RC PIDs for pitch (2 loops: angle --> angular velocity)
parameter_struct->pitch_angle_pid.dt = 0.005; parameter_struct->pitch_ang_vel_pid.dt = 0.005; // 5 ms calculation period
// initialize Yaw PID_t and PID constants
// RC PID for yaw (1 loop angular velocity)
parameter_struct->yaw_ang_vel_pid.dt = 0.005; // 5 ms calculation period
// AUTOMATIC Pilot (Position) PID DEFINITIONS //////
// Local X PID using a translation from X camera system data to quad local X position (3 loops: local y position --> angle --> angular velocity)
parameter_struct->local_x_pid.dt = 0.100;
// Local Y PID using a translation from Y camera system data to quad local Y position(3 loops: local x position --> angle --> angular velocity)
parameter_struct->local_y_pid.dt = 0.100;
// CAM PIDs for yaw (2 loops angle --> angular velocity)
parameter_struct->yaw_angle_pid.dt = 0.100;
// CAM PID for altitude (1 loop altitude)
parameter_struct->alt_pid.dt = 0.100;
// PID coeffiecients (Position)
setPIDCoeff(&(parameter_struct->local_y_pid), YPOS_KP, YPOS_KI, YPOS_KD);
setPIDCoeff(&(parameter_struct->local_x_pid), XPOS_KP, XPOS_KI, XPOS_KD);
setPIDCoeff(&(parameter_struct->alt_pid), ALT_ZPOS_KP, ALT_ZPOS_KI, ALT_ZPOS_KD);
// PID coefficients (Angle)
setPIDCoeff(&(parameter_struct->pitch_angle_pid), PITCH_ANGLE_KP, PITCH_ANGLE_KI, PITCH_ANGLE_KD);
setPIDCoeff(&(parameter_struct->roll_angle_pid), ROLL_ANGLE_KP, ROLL_ANGLE_KI, ROLL_ANGLE_KD);
setPIDCoeff(&(parameter_struct->yaw_angle_pid), YAW_ANGLE_KP, YAW_ANGLE_KI, YAW_ANGLE_KD);
// PID coefficients (Angular Velocity)
setPIDCoeff(&(parameter_struct->pitch_ang_vel_pid), PITCH_ANGULAR_VELOCITY_KP, PITCH_ANGULAR_VELOCITY_KI, PITCH_ANGULAR_VELOCITY_KD);
setPIDCoeff(&(parameter_struct->roll_ang_vel_pid), ROLL_ANGULAR_VELOCITY_KP, ROLL_ANGULAR_VELOCITY_KI, ROLL_ANGULAR_VELOCITY_KD);
setPIDCoeff(&(parameter_struct->yaw_ang_vel_pid), YAW_ANGULAR_VELOCITY_KP, YAW_ANGULAR_VELOCITY_KI, YAW_ANGULAR_VELOCITY_KD);
return 0;
}
int control_algorithm(log_t* log_struct, user_input_t * user_input_struct, sensor_t* sensor_struct, setpoint_t* setpoint_struct, parameter_t* parameter_struct, user_defined_t* user_defined_struct, raw_actuator_t* raw_actuator_struct, modular_structs_t* structs)
{
// use the 'flap' switch as the flight mode selector
int cur_fm_switch = read_flap(user_input_struct->rc_commands[FLAP]);
static int last_fm_switch = MANUAL_FLIGHT_MODE;
// reset flight_mode to MANUAL right away if the flap switch is in manual position
// to engage AUTO mode the code waits for a new packet after the flap is switched to auto
// before actually engaging AUTO mode
if(cur_fm_switch == MANUAL_FLIGHT_MODE)
user_defined_struct->flight_mode = MANUAL_FLIGHT_MODE;
static float roll_trim = 0.0;
static float pitch_trim = 0.0;
// flap switch was just toggled to auto flight mode
if((last_fm_switch != cur_fm_switch) && (cur_fm_switch == AUTO_FLIGHT_MODE))
{
user_defined_struct->engaging_auto = 1;
// Read in trimmed values because it should read trim values right when the pilot flips the flight mode switch
pitch_trim = user_input_struct->pitch_angle_manual_setpoint; //rc_commands[PITCH] - PITCH_CENTER;
roll_trim = user_input_struct->roll_angle_manual_setpoint; //rc_commands[ROLL] - ROLL_CENTER;
//sensor_struct->trimmedRCValues.yaw = yaw_manual_setpoint; //rc_commands[YAW] - YAW_CENTER;
// sensor_struct->trims.roll = raw_actuator_struct->controller_corrected_motor_commands[ROLL];
// sensor_struct->trims.pitch = raw_actuator_struct->controller_corrected_motor_commands[PITCH];
// sensor_struct->trims.yaw = raw_actuator_struct->controller_corrected_motor_commands[YAW];
sensor_struct->trims.throttle = user_input_struct->rc_commands[THROTTLE];
log_struct->trims.roll = sensor_struct->trims.roll;
log_struct->trims.pitch = sensor_struct->trims.pitch;
log_struct->trims.yaw = sensor_struct->trims.yaw;
log_struct->trims.throttle = sensor_struct->trims.throttle;
}
if(user_input_struct->locationFresh && user_defined_struct->engaging_auto == 1)
user_defined_struct->engaging_auto = 2;
// if the flap switch was toggled to AUTO_FLIGHT_MODE and we've received a new packet
// then record the current position as the desired position
// also reset the previous error and accumulated error from the position PIDs
if((cur_fm_switch == AUTO_FLIGHT_MODE) && (user_defined_struct->engaging_auto == 2))
{
// zero out the accumulated error so the I terms don't cause wild things to happen
parameter_struct->alt_pid.acc_error = 0.0;
parameter_struct->local_x_pid.acc_error = 0.0;
parameter_struct->local_y_pid.acc_error = 0.0;
// make previous error equal to the current so the D term doesn't spike
parameter_struct->alt_pid.prev_error = 0.0;
parameter_struct->local_x_pid.prev_error = 0.0;
parameter_struct->local_y_pid.prev_error = 0.0;
setpoint_struct->desiredQuadPosition.alt_pos = sensor_struct->currentQuadPosition.alt_pos;
setpoint_struct->desiredQuadPosition.x_pos = sensor_struct->currentQuadPosition.x_pos;
setpoint_struct->desiredQuadPosition.y_pos = sensor_struct->currentQuadPosition.y_pos;
setpoint_struct->desiredQuadPosition.yaw = 0.0;//currentQuadPosition.yaw;
// reset the flag that engages auto mode
user_defined_struct->engaging_auto = 0;
// finally engage the AUTO_FLIGHT_MODE
// this ensures that we've gotten a new update packet right after the switch was set to auto mode
user_defined_struct->flight_mode = AUTO_FLIGHT_MODE;
}
//PIDS///////////////////////////////////////////////////////////////////////
/* Position loop
* Reads current position, and outputs
* a pitch or roll for the angle loop PIDs
*/
// static int counter_between_packets = 0;
if(user_input_struct->locationFresh)
{
parameter_struct->local_y_pid.current_point = sensor_struct->currentQuadPosition.y_pos;
parameter_struct->local_y_pid.setpoint = setpoint_struct->desiredQuadPosition.y_pos;
parameter_struct->local_x_pid.current_point = sensor_struct->currentQuadPosition.x_pos;
parameter_struct->local_x_pid.setpoint = setpoint_struct->desiredQuadPosition.x_pos;
parameter_struct->alt_pid.current_point = sensor_struct->currentQuadPosition.alt_pos;
parameter_struct->alt_pid.setpoint = setpoint_struct->desiredQuadPosition.alt_pos;
//logging and PID computation
log_struct->local_y_PID_values = pid_computation(&(parameter_struct->local_y_pid));
log_struct->local_x_PID_values = pid_computation(&(parameter_struct->local_x_pid));
log_struct->altitude_PID_values = pid_computation(&(parameter_struct->alt_pid));
// yaw angular position PID calculation
parameter_struct->yaw_angle_pid.current_point = sensor_struct->currentQuadPosition.yaw;// in radians
parameter_struct->yaw_angle_pid.setpoint = setpoint_struct->desiredQuadPosition.yaw; // constant setpoint
//logging and PID computation
log_struct->angle_yaw_PID_values = pid_computation(&(parameter_struct->yaw_angle_pid));
}
/* Angle loop
* Calculates current orientation, and outputs
* a pitch, roll, or yaw velocity for the angular velocity loop PIDs
*/
//angle boundaries
if(parameter_struct->local_x_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
{
parameter_struct->local_x_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
}
if(parameter_struct->local_x_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
{
parameter_struct->local_x_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
}
if(parameter_struct->local_y_pid.pid_correction > ROLL_PITCH_MAX_ANGLE)
{
parameter_struct->local_y_pid.pid_correction = ROLL_PITCH_MAX_ANGLE;
}
if(parameter_struct->local_y_pid.pid_correction < -ROLL_PITCH_MAX_ANGLE)
{
parameter_struct->local_y_pid.pid_correction = -ROLL_PITCH_MAX_ANGLE;
}
parameter_struct->pitch_angle_pid.current_point = sensor_struct->pitch_angle_filtered;
parameter_struct->pitch_angle_pid.setpoint =
/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
(parameter_struct->local_x_pid.pid_correction) + pitch_trim :*/ user_input_struct->pitch_angle_manual_setpoint;
parameter_struct->roll_angle_pid.current_point = sensor_struct->roll_angle_filtered;
parameter_struct->roll_angle_pid.setpoint =
/*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
(parameter_struct->local_y_pid.pid_correction) + roll_trim :*/ user_input_struct->roll_angle_manual_setpoint;
//logging and PID computation
log_struct->angle_pitch_PID_values = pid_computation(&(parameter_struct->pitch_angle_pid));
log_struct->angle_roll_PID_values = pid_computation(&(parameter_struct->roll_angle_pid));
/* Angular Velocity Loop
* Takes the desired angular velocity from the angle loop,
* and calculates a PID correction with the current angular velocity
*/
// theta_dot is the angular velocity about the y-axis
// it is calculated from using the gimbal equations
parameter_struct->pitch_ang_vel_pid.current_point = sensor_struct->theta_dot;
parameter_struct->pitch_ang_vel_pid.setpoint = parameter_struct->pitch_angle_pid.pid_correction;
// phi_dot is the angular velocity about the x-axis
// it is calculated from using the gimbal equations
parameter_struct->roll_ang_vel_pid.current_point = sensor_struct->phi_dot;
parameter_struct->roll_ang_vel_pid.setpoint = parameter_struct->roll_angle_pid.pid_correction;
// Yaw angular velocity PID
// psi_dot is the angular velocity about the z-axis
// it is calculated from using the gimbal equations
parameter_struct->yaw_ang_vel_pid.current_point = sensor_struct->psi_dot;
parameter_struct->yaw_ang_vel_pid.setpoint = /*(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)?
parameter_struct->yaw_angle_pid.pid_correction :*/ user_input_struct->yaw_manual_setpoint; // no trim added because the controller already works well
//logging and PID computation
log_struct->ang_vel_pitch_PID_values = pid_computation(&(parameter_struct->pitch_ang_vel_pid));
log_struct->ang_vel_roll_PID_values = pid_computation(&(parameter_struct->roll_ang_vel_pid));
log_struct->ang_vel_yaw_PID_values = pid_computation(&(parameter_struct->yaw_ang_vel_pid));
//END PIDs///////////////////////////////////////////////////////////////////////
// here for now so in case any flight command is not PID controlled, it will default to rc_command value:
memcpy(raw_actuator_struct->controller_corrected_motor_commands, user_input_struct->rc_commands, sizeof(int) * 6);
// don't use the PID corrections if the throttle is less than about 10% of its range
if((user_input_struct->rc_commands[THROTTLE] >
118000) || (user_defined_struct->flight_mode == AUTO_FLIGHT_MODE))
{
if(user_defined_struct->flight_mode == AUTO_FLIGHT_MODE)
{
//THROTTLE
raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] =
((int)(parameter_struct->alt_pid.pid_correction)) + sensor_struct->trims.throttle;
//ROLL
raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
parameter_struct->roll_ang_vel_pid.pid_correction; // + sensor_struct->trims.roll;
//PITCH
raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
parameter_struct->pitch_ang_vel_pid.pid_correction; // + sensor_struct->trims.pitch;
//YAW
raw_actuator_struct->controller_corrected_motor_commands[YAW] =
parameter_struct->yaw_ang_vel_pid.pid_correction;// + sensor_struct->trims.yaw;
// static int slow_down = 0;
// slow_down++;
// if(slow_down % 50 == 0)
// printf("X: %.3f\tY: %.3f\tZ: %.3f\tX_s: %.3f\tX_c: %.3f\tY_s: %.3f\tY_c: %.3f\tZ_s: %.3f\tZ_c: %.3f\t\n",
// parameter_struct->local_x_pid.pid_correction,
// parameter_struct->local_y_pid.pid_correction,
// parameter_struct->alt_pid.pid_correction,
// parameter_struct->local_x_pid.setpoint, parameter_struct->local_x_pid.current_point,
// parameter_struct->local_y_pid.setpoint, parameter_struct->local_y_pid.current_point,
// parameter_struct->alt_pid.setpoint, parameter_struct->alt_pid.current_point);
}
else{
//ROLL
raw_actuator_struct->controller_corrected_motor_commands[ROLL] =
parameter_struct->roll_ang_vel_pid.pid_correction;
//PITCH
raw_actuator_struct->controller_corrected_motor_commands[PITCH] =
parameter_struct->pitch_ang_vel_pid.pid_correction;
//YAW
raw_actuator_struct->controller_corrected_motor_commands[YAW] =
parameter_struct->yaw_ang_vel_pid.pid_correction;
}
//BOUNDS CHECKING
if(raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] < 0)
raw_actuator_struct->controller_corrected_motor_commands[THROTTLE] = 0;
//BOUNDS CHECKING
if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] > 20000)
raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 20000;
if(raw_actuator_struct->controller_corrected_motor_commands[ROLL] < -20000)
raw_actuator_struct->controller_corrected_motor_commands[ROLL] = -20000;
if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] > 20000)
raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 20000;
if(raw_actuator_struct->controller_corrected_motor_commands[PITCH] < -20000)
raw_actuator_struct->controller_corrected_motor_commands[PITCH] = -20000;
if(raw_actuator_struct->controller_corrected_motor_commands[YAW] > 20000)
raw_actuator_struct->controller_corrected_motor_commands[YAW] = 20000;
if(raw_actuator_struct->controller_corrected_motor_commands[YAW] < -20000)
raw_actuator_struct->controller_corrected_motor_commands[YAW] = -20000;
}
else
{
raw_actuator_struct->controller_corrected_motor_commands[ROLL] = 0;
raw_actuator_struct->controller_corrected_motor_commands[PITCH] = 0;
raw_actuator_struct->controller_corrected_motor_commands[YAW] = 0;
}
//logging
// here we are not actually duplicating the logging from the PID computation
// the PID computation logs PID_values struct where this logs the PID struct
// they contain different sets of data
log_struct->local_y_PID = parameter_struct->local_y_pid;
log_struct->local_x_PID = parameter_struct->local_x_pid;
log_struct->altitude_PID = parameter_struct->alt_pid;
log_struct->angle_roll_PID = parameter_struct->roll_angle_pid;
log_struct->angle_pitch_PID = parameter_struct->pitch_angle_pid;
log_struct->angle_yaw_PID = parameter_struct->yaw_angle_pid;
log_struct->ang_vel_roll_PID = parameter_struct->roll_ang_vel_pid;
log_struct->ang_vel_pitch_PID = parameter_struct->pitch_ang_vel_pid;
log_struct->ang_vel_yaw_PID = parameter_struct->yaw_ang_vel_pid;
last_fm_switch = cur_fm_switch;
// Make location stale now
user_input_struct->locationFresh = 0;
return 0;
}
void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue) {
p->Kp = pValue;
p->Ki = iValue;
p->Kd = dValue;
}
quad/sw/modular_quad_pid/src/control_algorithm.h 0 → 100644
View file @ f6378157
/*
* control_algorithm.h
*
* Created on: Feb 20, 2016
* Author: ucart
*/
#ifndef CONTROL_ALGORITHM_H_
#define CONTROL_ALGORITHM_H_
#include <stdio.h>
#include "log_data.h"
#include "sensor_processing.h"
#include "quadposition.h"
#include "type_def.h"
/**
* @brief
* Initializes everything used in the control algorithm.
*
* @return
* error message
*
*/
int control_algorithm_init(parameter_t * parameter_struct);
/**
* @brief
* Runs the control algorithm on the data and outputs a command for actuators.
*
* @param log_struct
* structure of the data to be logged
*
* @param sensor_struct
* structure of the processed data from the sensors
*
* @param setpoint_struct
* structure of the setpoints used in the controller
*
* @param parameter_struct
* structure of the parameters used in the controller
*
* @param user_defined_struct
* structure of the user defined variables
*
* @param raw_actuator_struct
* structure of the commmands outputted to go to the actuators
*
* @return
* error message
*
*/
int control_algorithm(log_t* log_struct,
user_input_t * user_input_struct,
sensor_t* sensor_struct,
setpoint_t* setpoint_struct,
parameter_t* parameter_struct,
user_defined_t* user_defined_struct,
raw_actuator_t* raw_actuator_struct,
modular_structs_t* structs);
/**
* @brief
* Internally used functions
*
*/
void setPIDCoeff(PID_t* p, float pValue, float iValue, float dValue);
#endif /* CONTROL_ALGORITHM_H_ */
quad/sw/modular_quad_pid/src/controllers.c 0 → 100644
View file @ f6378157
/*
* controllers.c
*
* Created on: Oct 11, 2014
* Author: ucart
*/
/**
* Lots of useful information in controllers.h, look in there first
*/
#include "controllers.h"
#include "iic_mpu9150_utils.h"
#include "quadposition.h"
#include "util.h"
#include "uart.h"
#include "sleep.h"
#include "stdio.h"
#include <math.h>
// 0 was -6600
//int motor0_bias = -4500, motor1_bias = 100, motor2_bias = 5300, motor3_bias = 10300;
int motor0_bias = -9900, motor1_bias = -200, motor2_bias = -10200, motor3_bias = 250;
/**
* Takes the raw signal inputs from the receiver and filters it so the
* quadcopter doesn't flip or do something extreme
*/
void filter_PWMs(int* mixer) {
}
/**
* Converts PWM signals into 4 channel pitch, roll, yaw, throttle
*/
// javey: unused
void PWMS_to_Aero(int* PWMs, int* aero) {
/**
* Reference used to derive equations
*/
// pwm0 = throttle_base - pitch_base + yaw_base;
// pwm1 = throttle_base + roll_base - yaw_base;
// pwm2 = throttle_base - roll_base - yaw_base;
// pwm3 = throttle_base + pitch_base + yaw_base;
aero[THROTTLE] = (PWMs[0] + PWMs[1] + PWMs[2] + PWMs[3]) / 4;
aero[ROLL] = (PWMs[1] - PWMs[2]) / 2;
aero[PITCH] = (PWMs[3] - PWMs[0]) / 2;
aero[YAW] = (PWMs[3] + PWMs[0] - PWMs[1] - PWMs[2]) / 4;
}
quad/sw/modular_quad_pid/src/controllers.h 0 → 100644
View file @ f6378157
/*
* controllers.h
*
* Created on: Oct 11, 2014
* Author: ucart
*/
#ifndef _CONTROLLERS_H
#define _CONTROLLERS_H
#include "util.h"
#include "quadposition.h"
/**
*
* USING PLUS CONFIGURATION
* 0 R CW E
* 1 + 2 W R CCW CCW N S
* 3 W CW W
*
*
* USING X CONFIGURATION
*
*
* 0 2 R R CW CCW
* x
* 1 3 W W CCW CW
*/
#define X_CONFIG
/**
* Pin hook ups
*
* PWM Recorder port mapping
* 3.3 V || GND || PWM_REC_3 || PWM_REC_2 || PWM_REC_1 || PWM_REC_0
*
* Rx PINS
* GEAR -> JD7
* THROTTLE -> JE1
* AILE -> JE2
* ELEV -> JE3
* RUDD -> JE4
* GND -> JE5
*
* JE PMOD TOP PINS
* Unused || GND || YAW || PITCH || ROLL || THROTTLE
*
* BOTTOM PINS
*
* Unused || GND || PWM3 || PWM2 || PWM1 || PWM0
*/
/**
* Gear settings
* 1 - F mode = 171135
* 0 - Gear = 118363
* Kill if gear is around 118363
*/
/*
* Aero channel declaration
*/
#define THROTTLE 0
#define ROLL 1
#define PITCH 2
#define YAW 3
#define GEAR 4
#define FLAP 5
/**
* Signals from the Rx mins, maxes and ranges
*/
#define THROTTLE_MAX 191900
#define THROTTLE_MIN 110200
#define THROTTLE_RANGE THROTTLE_MAX - THROTTLE_MIN
#define ROLL_MAX 170200
#define ROLL_MIN 129400
#define ROLL_CENTER 149800
#define ROLL_RANGE ROLL_MAX - ROLL_MIN
#define PITCH_MAX 169900
#define PITCH_MIN 129500
#define PITCH_CENTER 149700
#define PITCH_RANGE PITCH_MAX - PITCH_MIN
#define YAW_MAX 169400
#define YAW_MIN 129300
#define YAW_CENTER 149800
#define YAW_RANGE YAW_MAX - YAW_MIN
#define GEAR_1 170800
#define GEAR_0 118300
#define FLAP_1 192000
#define FLAP_0 107600
#define GEAR_KILL GEAR_0 // The kill point for the program
#define BASE 150000
#define min 100000
#define max 200000
void filter_PWMs(int* mixer);
void PWMS_to_Aero(int* PWMs, int* aero); // <= javey: unused
void Aero_to_PWMS(int* PWMs, int* aero);
#endif /* _CONTROLLERS_H */
quad/sw/modular_quad_pid/src/conversion.c 0 → 100644
View file @ f6378157
#include "conversion.h"
// takes a floating point percentage and converts to a
// receiver command in the range min_rx_cmd to max_rx_cmd
// if percentage is < 0 then returns a value less than
// center_rx_cmd but >= min_rx_cmd
// if percentage is > 0 then returns a value greater than
// center_rx_cmd but <= max_rx_cmd
// if percentage is = 0 then returns center_rx_cmd
// acceptable range of values for percentage: [-100, 100]
int map_to_rx_cmd(float percentage, int min_rx_cmd, int center_rx_cmd,
int max_rx_cmd)
{
//bounds checking
// imagine a human flying and the stick is minimum
if(percentage >= 100.0)
return max_rx_cmd;
//bounds checking
// imagine a human flying and the stick is minimum
if(percentage <= -100.0)
return min_rx_cmd;
// 0 percentage is center cmd
// imagine a human flying and not touching the stick
if(percentage == 0)
return center_rx_cmd;
// calculate and return a percentage of the max/min command
if(percentage < 0)
{
return center_rx_cmd + ((int) (percentage/100.0 *
((float) max_rx_cmd - center_rx_cmd)));
}
else
{
return center_rx_cmd + ((int) (percentage/100.0 * (
(float) center_rx_cmd - min_rx_cmd)));
}
return 0;
}
int convert_to_receiver_cmd(int var_to_convert, float max_var_to_convert, float min_var_to_convert, int center_receiver_cmd, int max_receiver_cmd, int min_receiver_cmd)
{
if(var_to_convert <= 0) {
int ret = ((int) ((float)(min_receiver_cmd - center_receiver_cmd))/min_var_to_convert * var_to_convert) + center_receiver_cmd;
if(ret < min_receiver_cmd)
ret = min_receiver_cmd;
return ret;
}
else {
int ret = ((int) ((float)(max_receiver_cmd - center_receiver_cmd))/max_var_to_convert * var_to_convert) + center_receiver_cmd;
if(ret > max_receiver_cmd)
ret = max_receiver_cmd;
return ret;
}
return 0;
}
quad/sw/modular_quad_pid/src/conversion.h 0 → 100644
View file @ f6378157
#ifndef _CONVERSION_H
#define _CONVERSION_H
int convert_to_receiver_cmd(int var_to_convert, float max_var_to_convert, float min_var_to_convert, int center_receiver_cmd, int max_receiver_cmd, int min_receiver_cmd);
int map_to_rx_cmd(float percentage, int min_rx_cmd, int center_rx_cmd, int max_rx_cmd);
#endif /* _CONVERSION_H */