mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-30 11:37:06 +08:00
[sd logger] Version 2.0 release, direct SPI logging, UART download
This commit is contained in:
Bart Slinger
@@ -36,6 +36,13 @@ paparazzi.sublime-workspace
|
|||||||
# JetBrains (PyCharm, etc) IDE project files
|
# JetBrains (PyCharm, etc) IDE project files
|
||||||
.idea
|
.idea
|
||||||
|
|
||||||
|
# QtCreator IDE
|
||||||
|
*.creator
|
||||||
|
*.creator.user
|
||||||
|
*.config
|
||||||
|
*.files
|
||||||
|
*.includes
|
||||||
|
|
||||||
# Vagrant VM files
|
# Vagrant VM files
|
||||||
/.vagrant
|
/.vagrant
|
||||||
|
|
||||||
@@ -126,6 +133,7 @@ paparazzi.sublime-workspace
|
|||||||
/sw/logalizer/openlog2tlm
|
/sw/logalizer/openlog2tlm
|
||||||
/sw/logalizer/ahrs2fg
|
/sw/logalizer/ahrs2fg
|
||||||
/sw/logalizer/disp3d
|
/sw/logalizer/disp3d
|
||||||
|
/sw/logalizer/sdlogger_download
|
||||||
|
|
||||||
# /sw/simulator/
|
# /sw/simulator/
|
||||||
/sw/simulator/gaia
|
/sw/simulator/gaia
|
||||||
|
|||||||
@@ -0,0 +1,230 @@
|
|||||||
|
<!-- this is a Walkera Genius V2 frame equiped with Lisa/S 0.1 with brushless main rotor -->
|
||||||
|
<!-- Using Lisa/S V1.0 board file as it is software compatible. -->
|
||||||
|
|
||||||
|
<!--
|
||||||
|
Applicable configuration:
|
||||||
|
airframe="airframes/TUDelft/walkera_genius_v2.xml"
|
||||||
|
radio="radios/cockpitSX.xml"
|
||||||
|
telemetry="telemetry/default_rotorcraft.xml"
|
||||||
|
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||||
|
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml"
|
||||||
|
-->
|
||||||
|
|
||||||
|
<airframe name="walkera_genius_v2">
|
||||||
|
|
||||||
|
<servos driver="Pwm">
|
||||||
|
<servo name="CIC_FRONT" no="4" min="1000" neutral="1400" max="1700"/>
|
||||||
|
<servo name="CIC_RIGHT" no="5" min="1150" neutral="1450" max="1850"/>
|
||||||
|
<servo name="CIC_LEFT" no="0" min="1000" neutral="1400" max="1700"/>
|
||||||
|
<servo name="TAIL" no="2" min="0" neutral="0" max="500"/>
|
||||||
|
<servo name="GAS" no="3" min="1890" neutral="1890" max="1430"/>
|
||||||
|
</servos>
|
||||||
|
|
||||||
|
<commands>
|
||||||
|
<axis name="PITCH" failsafe_value="0"/>
|
||||||
|
<axis name="ROLL" failsafe_value="0"/>
|
||||||
|
<axis name="YAW" failsafe_value="0"/>
|
||||||
|
<axis name="THRUST" failsafe_value="0"/>
|
||||||
|
</commands>
|
||||||
|
|
||||||
|
<command_laws>
|
||||||
|
<let var="hoverpower" value=".85*MAX_PPRZ"/>
|
||||||
|
<let var="hoverstick" value=".35*MAX_PPRZ"/>
|
||||||
|
<let var="halfway" value="(@THRUST >= ($hoverstick) ? 1 : 0)"/>
|
||||||
|
<let var="collective" value="@THRUST * 0.1258 + (MAX_PPRZ*0.2013)"/>
|
||||||
|
<!--let var="collective" value="@PITCH"/-->
|
||||||
|
<let var="gas" value="($hoverpower) + $halfway * (((@THRUST) - ($hoverstick)) * (MAX_PPRZ - ($hoverpower))) / (MAX_PPRZ - ($hoverstick)) + (1 - $halfway) * ((@THRUST - ($hoverstick)) * ($hoverpower)) / ($hoverstick)"/>
|
||||||
|
|
||||||
|
<set servo="CIC_LEFT" value="((@PITCH*1.75*0.5*0.5)-(@ROLL*1.75*0.5*0.73))-($collective)"/>
|
||||||
|
<set servo="CIC_RIGHT" value="((-@PITCH*1.75*0.5*0.5)-(@ROLL*1.75*0.5*0.73))+($collective)"/>
|
||||||
|
<set servo="CIC_FRONT" value="-@PITCH*1.75*0.5-($collective)"/>
|
||||||
|
|
||||||
|
<!--set servo="CIC_LEFT" value="-$collective"/>
|
||||||
|
<set servo="CIC_RIGHT" value="$collective"/>
|
||||||
|
<set servo="CIC_FRONT" value="-$collective"/-->
|
||||||
|
|
||||||
|
<set servo="GAS" value="$gas"/>
|
||||||
|
<set servo="TAIL" value="@YAW + $halfway * ((MAX_PPRZ*0.4) + @THRUST*0.2037) + (1 - $halfway) * (@THRUST * 1.4872) "/>
|
||||||
|
<!--set servo="TAIL" value="0"/-->
|
||||||
|
|
||||||
|
<!--set servo="GAS" value="$gas"/>
|
||||||
|
<set servo="TAIL" value="@YAW+0.4*$gas"/-->
|
||||||
|
|
||||||
|
</command_laws>
|
||||||
|
|
||||||
|
<section name="IMU" prefix="IMU_">
|
||||||
|
<define name="BODY_TO_IMU_PHI" value="184." unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_THETA" value="-4.5" unit="deg"/>
|
||||||
|
<define name="BODY_TO_IMU_PSI" value="0" unit="deg"/>
|
||||||
|
|
||||||
|
<!-- From Delft MAVlab, quick calibration -->
|
||||||
|
<define name="ACCEL_X_NEUTRAL" value="0"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="0"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="0"/>
|
||||||
|
<!--define name="ACCEL_X_NEUTRAL" value="62"/>
|
||||||
|
<define name="ACCEL_Y_NEUTRAL" value="16"/>
|
||||||
|
<define name="ACCEL_Z_NEUTRAL" value="-191"/>
|
||||||
|
<define name="ACCEL_X_SENS" value="4.86205019187" integer="16"/>
|
||||||
|
<define name="ACCEL_Y_SENS" value="4.88606377201" integer="16"/>
|
||||||
|
<define name="ACCEL_Z_SENS" value="4.84406734807" integer="16"/-->
|
||||||
|
|
||||||
|
|
||||||
|
<!-- From delft MAVLab -->
|
||||||
|
<define name="MAG_X_NEUTRAL" value="396"/>
|
||||||
|
<define name="MAG_Y_NEUTRAL" value="-260"/>
|
||||||
|
<define name="MAG_Z_NEUTRAL" value="-91"/>
|
||||||
|
<define name="MAG_X_SENS" value="3.71635905504" integer="16"/>
|
||||||
|
<define name="MAG_Y_SENS" value="3.5526454063" integer="16"/>
|
||||||
|
<define name="MAG_Z_SENS" value="3.52339566014" integer="16"/>
|
||||||
|
|
||||||
|
<!-- Current correction -->
|
||||||
|
<define name="MAG_X_CURRENT_COEF" value="-0.390312445109"/>
|
||||||
|
<define name="MAG_Y_CURRENT_COEF" value="-0.068976232866"/>
|
||||||
|
<define name="MAG_Z_CURRENT_COEF" value="-0.181415585657"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="AUTOPILOT">
|
||||||
|
<define name="MODE_AUTO2" value="AP_MODE_RC_DIRECT"/> <!--AP_MODE_ATTITUDE_DIRECT-->
|
||||||
|
<define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_DIRECT"/>
|
||||||
|
<define name="MODE_MANUAL" value="AP_MODE_KILL"/> <!--AP_MODE_ATTITUDE_Z_HOLD-->
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="BAT">
|
||||||
|
<define name="MILLIAMP_AT_FULL_THROTTLE" value="3700"/>
|
||||||
|
<define name="CATASTROPHIC_BAT_LEVEL" value="3.0" unit="V"/>
|
||||||
|
<define name="CRITIC_BAT_LEVEL" value="3.2" unit="V"/>
|
||||||
|
<define name="LOW_BAT_LEVEL" value="3.5" unit="V"/>
|
||||||
|
<define name="MAX_BAT_LEVEL" value="4.2" unit="V"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
|
||||||
|
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
|
||||||
|
<define name="SP_MAX_P" value="80000"/>
|
||||||
|
<define name="SP_MAX_Q" value="80000"/>
|
||||||
|
<define name="SP_MAX_R" value="80000"/>
|
||||||
|
|
||||||
|
<define name="GAIN_P" value="400"/>
|
||||||
|
<define name="GAIN_Q" value="400"/>
|
||||||
|
<define name="GAIN_R" value="350"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
|
||||||
|
|
||||||
|
<!-- setpoints -->
|
||||||
|
<define name="SP_MAX_PHI" value="45." unit="deg"/>
|
||||||
|
<define name="SP_MAX_THETA" value="45." unit="deg"/>
|
||||||
|
<define name="SP_MAX_R" value="90." unit="deg/s"/>
|
||||||
|
<define name="DEADBAND_R" value="250"/>
|
||||||
|
<define name="DEADBAND_A" value="250"/>
|
||||||
|
|
||||||
|
<!-- reference -->
|
||||||
|
<define name="REF_OMEGA_P" value="3000" unit="deg/s"/>
|
||||||
|
<define name="REF_ZETA_P" value="0.85"/>
|
||||||
|
<define name="REF_MAX_P" value="300." unit="deg/s"/>
|
||||||
|
<define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>
|
||||||
|
|
||||||
|
<define name="REF_OMEGA_Q" value="3000" unit="deg/s"/>
|
||||||
|
<define name="REF_ZETA_Q" value="0.85"/>
|
||||||
|
<define name="REF_MAX_Q" value="300." unit="deg/s"/>
|
||||||
|
<define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>
|
||||||
|
|
||||||
|
<define name="REF_OMEGA_R" value="3000" unit="deg/s"/>
|
||||||
|
<define name="REF_ZETA_R" value="0.85"/>
|
||||||
|
<define name="REF_MAX_R" value="300." unit="deg/s"/>
|
||||||
|
<define name="REF_MAX_RDOT" value="RadOfDeg(7000.)"/>
|
||||||
|
|
||||||
|
<!-- feedback -->
|
||||||
|
<define name="PHI_PGAIN" value="3052"/>
|
||||||
|
<define name="PHI_DGAIN" value="108"/>
|
||||||
|
<define name="PHI_IGAIN" value="0"/>
|
||||||
|
|
||||||
|
<define name="THETA_PGAIN" value="3052"/>
|
||||||
|
<define name="THETA_DGAIN" value="108"/>
|
||||||
|
<define name="THETA_IGAIN" value="0"/>
|
||||||
|
|
||||||
|
<define name="PSI_PGAIN" value="944"/>
|
||||||
|
<define name="PSI_DGAIN" value="300"/>
|
||||||
|
<define name="PSI_IGAIN" value="10"/>
|
||||||
|
|
||||||
|
<!-- feedforward -->
|
||||||
|
<define name="PHI_DDGAIN" value="0"/>
|
||||||
|
<define name="THETA_DDGAIN" value="0"/>
|
||||||
|
<define name="PSI_DDGAIN" value=" 300"/>
|
||||||
|
|
||||||
|
<define name="PHI_AGAIN" value="0"/>
|
||||||
|
<define name="THETA_AGAIN" value="0"/>
|
||||||
|
<define name="PSI_AGAIN" value="0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="INS" prefix="INS_">
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GUIDANCE_V" prefix="GUIDANCE_V_">
|
||||||
|
<define name="HOVER_KP" value="281"/>
|
||||||
|
<define name="HOVER_KD" value="66"/>
|
||||||
|
<define name="HOVER_KI" value="0"/>
|
||||||
|
<define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value="0.9"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="AHRS" prefix="AHRS_">
|
||||||
|
<define name="H_X" value=" 0.47577"/>
|
||||||
|
<define name="H_Y" value=" 0.11811"/>
|
||||||
|
<define name="H_Z" value=" 0.87161"/>
|
||||||
|
<define name="USE_RPM_SENSOR_NOTCH" value="1"/>
|
||||||
|
<define name="NOTCH_FILTER_BANDWIDTH" value="10.0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="GUIDANCE_H" prefix="GUIDANCE_H_">
|
||||||
|
<define name="PGAIN" value="50"/>
|
||||||
|
<define name="DGAIN" value="50"/>
|
||||||
|
<define name="IGAIN" value="0"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section name="RPM_SENSOR" prefix="RPM_SENSOR_">
|
||||||
|
<define name="PULSES_PER_ROTATION" value="6"/>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<modules main_freq="512">
|
||||||
|
<load name="gps_ubx_ucenter.xml"/>
|
||||||
|
<load name="send_imu_mag_current.xml"/>
|
||||||
|
<load name="logger_sd_spi_direct.xml">
|
||||||
|
<configure name="LOGGER_CONTROL_SWITCH" value="RADIO_AUX2"/>
|
||||||
|
<configure name="LOGGER_LED" value="3"/>
|
||||||
|
</load>
|
||||||
|
<!--load name="sys_mon.xml"/-->
|
||||||
|
</modules>
|
||||||
|
|
||||||
|
<firmware name="rotorcraft">
|
||||||
|
<target name="ap" board="lisa_s_1.0">
|
||||||
|
<!--subsystem name="radio_control" type="superbitrf_rc"-->
|
||||||
|
<!--<define name="RADIO_TRANSMITTER_ID" value="2008496626"/>--> <!-- TUDelft Dx6i: TX 4 -->
|
||||||
|
<!--<define name="RADIO_TRANSMITTER_CHAN" value="6"/>-->
|
||||||
|
<!--<define name="RADIO_TRANSMITTER_PROTOCOL" value="0x01"/>-->
|
||||||
|
|
||||||
|
<subsystem name="radio_control" type="ppm">
|
||||||
|
<configure name="RADIO_CONTROL_PPM_PIN" value="SERVO6"/>
|
||||||
|
</subsystem>
|
||||||
|
|
||||||
|
<configure name="AHRS_PROPAGATE_FREQUENCY" value="500"/>
|
||||||
|
<define name="USE_PERSISTENT_SETTINGS" value="TRUE"/>
|
||||||
|
</target>
|
||||||
|
|
||||||
|
<subsystem name="actuators" type="pwm">
|
||||||
|
<define name="TIM5_SERVO_HZ" value="2000"/>
|
||||||
|
<define name="SERVO_HZ" value="300"/>
|
||||||
|
<define name="USE_SERVOS_1AND2"/>
|
||||||
|
</subsystem>
|
||||||
|
|
||||||
|
<subsystem name="telemetry" type="transparent">
|
||||||
|
<configure name="MODEM_BAUD" value="B115200"/>
|
||||||
|
<configure name="MODEM_PORT" value="UART1"/>
|
||||||
|
</subsystem>
|
||||||
|
<!--subsystem name="telemetry" type="superbitrf" /-->
|
||||||
|
<subsystem name="imu" type="lisa_s_v1.0"/>
|
||||||
|
<subsystem name="gps" type="ublox"/>
|
||||||
|
<subsystem name="stabilization" type="int_quat"/>
|
||||||
|
<!--subsystem name="rpm_sensor" type="eagletree"/-->
|
||||||
|
<subsystem name="ahrs" type="int_cmpl_quat"/>
|
||||||
|
<subsystem name="ins"/>
|
||||||
|
</firmware>
|
||||||
|
</airframe>
|
||||||
@@ -87,6 +87,17 @@
|
|||||||
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
|
settings_modules="modules/geo_mag.xml modules/air_data.xml modules/video_thread.xml modules/video_rtp_stream.xml"
|
||||||
gui_color="red"
|
gui_color="red"
|
||||||
/>
|
/>
|
||||||
|
<aircraft
|
||||||
|
name="Helisa"
|
||||||
|
ac_id="114"
|
||||||
|
airframe="airframes/TUDelft/airframes/walkera_genius_v2.xml"
|
||||||
|
radio="radios/FlyElectricRx31_DX6.xml"
|
||||||
|
telemetry="telemetry/rotorcraft_with_logger.xml"
|
||||||
|
flight_plan="flight_plans/rotorcraft_basic.xml"
|
||||||
|
settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_att_int.xml settings/control/stabilization_rate.xml [settings/superbitrf.xml]"
|
||||||
|
settings_modules="modules/gps_ubx_ucenter.xml modules/logger_sd_spi_direct.xml"
|
||||||
|
gui_color="#063982530000"
|
||||||
|
/>
|
||||||
<aircraft
|
<aircraft
|
||||||
name="MAVTec3_Chris"
|
name="MAVTec3_Chris"
|
||||||
ac_id="33"
|
ac_id="33"
|
||||||
|
|||||||
@@ -0,0 +1,65 @@
|
|||||||
|
<!DOCTYPE module SYSTEM "module.dtd">
|
||||||
|
|
||||||
|
<module name="logger_sd_spi_direct" dir="loggers">
|
||||||
|
<doc>
|
||||||
|
<description>Direct SPI SD Logger that saves pprzlog messages to SD Card.</description>
|
||||||
|
<configure name="SDLOGGER_DIRECT_SPI" value="SPI1|SPI2|SPI3|SPI4|SPI5|SPI6" description="Port to which the SD Card is connected."/>
|
||||||
|
<configure name="SDLOGGER_DIRECT_SPI_SLAVE" value="SPI_SLAVE1|SPI_SLAVE2|SPI_SLAVE3|SPI_SLAVE4|SPI_SLAVE5|SPI_SLAVE6" description="Port to which the SD Card is connected."/>
|
||||||
|
<configure name="LOGGER_CONTROL_SWITCH" value="RADIO_AUX2"/>
|
||||||
|
<configure name="LOGGER_LED" value="none"/>
|
||||||
|
</doc>
|
||||||
|
<settings>
|
||||||
|
<dl_settings NAME="sdlogger">
|
||||||
|
<dl_settings NAME="SD Logger">
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdlogger_spi.status" min="0" max="20" step="1"/>
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdcard1.status" min="0" max="40" step="1"/>
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdlogger_spi.last_completed" min="0" max="42" step="1"/>
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdlogger_spi.next_available_address" min="16384" max="4294967295" step="1" />
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdlogger_spi.command" min="1" max="42" step="1" />
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdlogger_spi.download_address" min="0" max="4294967295" step="1"/>
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdlogger_spi.download_length" min="0" max="4294967295" step="1"/>
|
||||||
|
<dl_setting module="loggers/sdlogger_spi_direct" var="sdlogger_spi.download_id" min="0" max="255" step="1"/>
|
||||||
|
</dl_settings>
|
||||||
|
</dl_settings>
|
||||||
|
</settings>
|
||||||
|
<header>
|
||||||
|
<file name="sdlogger_spi_direct.h"/>
|
||||||
|
</header>
|
||||||
|
<init fun="sdlogger_spi_direct_init()"/>
|
||||||
|
<periodic fun="sdlogger_spi_direct_periodic()" freq="512" start="sdlogger_spi_direct_start()" stop="sdlogger_spi_direct_stop()" autorun="TRUE"/>
|
||||||
|
<datalink message="SETTING" fun="sdlogger_spi_direct_command()"/>
|
||||||
|
<makefile>
|
||||||
|
|
||||||
|
<raw>
|
||||||
|
SDLOGGER_DIRECT_SPI ?= spi2
|
||||||
|
SDLOGGER_DIRECT_SPI_LOWER=$(shell echo $(SDLOGGER_DIRECT_SPI) | tr A-Z a-z)
|
||||||
|
SDLOGGER_DIRECT_SPI_UPPER=$(shell echo $(SDLOGGER_DIRECT_SPI) | tr a-z A-Z)
|
||||||
|
|
||||||
|
SDLOGGER_DIRECT_SPI_SLAVE ?= spi_slave2
|
||||||
|
SDLOGGER_DIRECT_SPI_SLAVE_LOWER=$(shell echo $(SDLOGGER_DIRECT_SPI_SLAVE) | tr A-Z a-z)
|
||||||
|
SDLOGGER_DIRECT_SPI_SLAVE_UPPER=$(shell echo $(SDLOGGER_DIRECT_SPI_SLAVE) | tr a-z A-Z)
|
||||||
|
|
||||||
|
LOGGER_CONTROL_SWITCH ?= RADIO_AUX2
|
||||||
|
|
||||||
|
LOGGER_LED ?= none
|
||||||
|
ifneq ($(LOGGER_LED),none)
|
||||||
|
ap.CFLAGS += -DLOGGER_LED=$(LOGGER_LED)
|
||||||
|
endif
|
||||||
|
|
||||||
|
ap.srcs += subsystems/datalink/downlink.c subsystems/datalink/pprzlog_transport.c
|
||||||
|
</raw>
|
||||||
|
|
||||||
|
<file name="sdcard_spi.c" dir="peripherals"/>
|
||||||
|
<file name="sdlogger_spi_direct.c"/>
|
||||||
|
</makefile>
|
||||||
|
<makefile target="ap">
|
||||||
|
<define name="USE_SDLOGGER_SPI_DIRECT" value="1" />
|
||||||
|
<define name="SPI_MASTER" value="1" />
|
||||||
|
<define name="USE_$(SDLOGGER_DIRECT_SPI_UPPER)" value="1" />
|
||||||
|
<define name="SDLOGGER_SPI_LINK_DEVICE" value="$(SDLOGGER_DIRECT_SPI_LOWER)" />
|
||||||
|
<define name="USE_$(SDLOGGER_DIRECT_SPI_SLAVE_UPPER)" value="1" />
|
||||||
|
<define name="SDLOGGER_SPI_LINK_SLAVE_NUMBER" value="$(SDLOGGER_DIRECT_SPI_SLAVE_UPPER)" />
|
||||||
|
<define name="SDLOGGER_CONTROL_SWITCH" value="$(LOGGER_CONTROL_SWITCH)"/>
|
||||||
|
</makefile>
|
||||||
|
</module>
|
||||||
|
|
||||||
@@ -0,0 +1,160 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<!DOCTYPE telemetry SYSTEM "telemetry.dtd">
|
||||||
|
<telemetry>
|
||||||
|
|
||||||
|
|
||||||
|
<process name="Main">
|
||||||
|
|
||||||
|
<mode name="default" key_press="d">
|
||||||
|
<message name="AUTOPILOT_VERSION" period="11.1"/>
|
||||||
|
<message name="DL_VALUE" period="1.1"/>
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="ROTORCRAFT_FP" period="0.25"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="INS_REF" period="5.1"/>
|
||||||
|
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
|
||||||
|
<message name="WP_MOVED" period="1.3"/>
|
||||||
|
<message name="ROTORCRAFT_CAM" period="1."/>
|
||||||
|
<message name="GPS_INT" period=".25"/>
|
||||||
|
<message name="INS" period=".25"/>
|
||||||
|
<message name="I2C_ERRORS" period="4.1"/>
|
||||||
|
<message name="UART_ERRORS" period="3.1"/>
|
||||||
|
<message name="SUPERBITRF" period="3"/>
|
||||||
|
<message name="ENERGY" period="2.5"/>
|
||||||
|
<message name="DATALINK_REPORT" period="5.1"/>
|
||||||
|
<message name="STATE_FILTER_STATUS" period="3.2"/>
|
||||||
|
<message name="AIR_DATA" period="1.3"/>
|
||||||
|
<message name="SURVEY" period="2.5"/>
|
||||||
|
<message name="OPTIC_FLOW_EST" period="0.25"/>
|
||||||
|
<message name="VECTORNAV_INFO" period="0.5"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="ppm">
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="ROTORCRAFT_CMD" period=".05"/>
|
||||||
|
<message name="PPM" period="0.5"/>
|
||||||
|
<message name="RC" period="0.5"/>
|
||||||
|
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.5"/>
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1"/>
|
||||||
|
<message name="ACTUATORS_BEBOP" period="0.2"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="raw_sensors">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="IMU_ACCEL_RAW" period=".05"/>
|
||||||
|
<message name="IMU_GYRO_RAW" period=".05"/>
|
||||||
|
<message name="IMU_MAG_RAW" period=".05"/>
|
||||||
|
<message name="BARO_RAW" period=".1"/>
|
||||||
|
<message name="ARDRONE_NAVDATA" period=".05"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="scaled_sensors">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="IMU_GYRO_SCALED" period=".075"/>
|
||||||
|
<message name="IMU_ACCEL_SCALED" period=".075"/>
|
||||||
|
<message name="IMU_MAG_SCALED" period=".1"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="ahrs">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="FILTER_ALIGNER" period="2.2"/>
|
||||||
|
<message name="FILTER" period=".5"/>
|
||||||
|
<message name="GEO_MAG" period="5."/>
|
||||||
|
<message name="AHRS_GYRO_BIAS_INT" period="0.08"/>
|
||||||
|
<message name="AHRS_QUAT_INT" period=".25"/>
|
||||||
|
<message name="AHRS_EULER_INT" period=".1"/>
|
||||||
|
<!-- <message name="AHRS_RMAT_INT" period=".5"/> -->
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="rate_loop">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="RATE_LOOP" period=".02"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="attitude_setpoint_viz" key_press="v">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="0.9"/>
|
||||||
|
<message name="ROTORCRAFT_RADIO_CONTROL" period="0.1"/>
|
||||||
|
<message name="AHRS_REF_QUAT" period="0.05"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="attitude_loop" key_press="a">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="0.9"/>
|
||||||
|
<message name="STAB_ATTITUDE" period=".03"/>
|
||||||
|
<message name="STAB_ATTITUDE_REF" period=".03"/>
|
||||||
|
<message name="STAB_ATTITUDE_INDI" period=".25"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="vert_loop" key_press="v">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="0.9"/>
|
||||||
|
<message name="VFF" period=".05"/>
|
||||||
|
<message name="VFF_EXTENDED" period=".05"/>
|
||||||
|
<message name="VERT_LOOP" period=".05"/>
|
||||||
|
<message name="INS_Z" period=".05"/>
|
||||||
|
<message name="INS" period=".11"/>
|
||||||
|
<message name="INS_REF" period="5.1"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="h_loop" key_press="h">
|
||||||
|
<message name="ALIVE" period="0.9"/>
|
||||||
|
<message name="HOVER_LOOP" period="0.062"/>
|
||||||
|
<message name="GUIDANCE_H_REF" period="0.062"/>
|
||||||
|
<message name="STAB_ATTITUDE" period="0.4"/>
|
||||||
|
<!--<message name="STAB_ATTITUDE_REF" period="0.4"/>-->
|
||||||
|
<message name="ROTORCRAFT_FP" period="0.8"/>
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="ROTORCRAFT_NAV_STATUS" period="1.6"/>
|
||||||
|
<message name="INS_REF" period="5.1"/>
|
||||||
|
<!-- HFF messages are only sent if USE_HFF -->
|
||||||
|
<message name="HFF" period=".05"/>
|
||||||
|
<message name="HFF_GPS" period=".03"/>
|
||||||
|
<message name="HFF_DBG" period=".2"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="aligner">
|
||||||
|
<message name="ALIVE" period="0.9"/>
|
||||||
|
<message name="FILTER_ALIGNER" period="0.02"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="tune_hover">
|
||||||
|
<message name="DL_VALUE" period="1.1"/>
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="GUIDANCE_H_INT" period="0.05"/>
|
||||||
|
<message name="ROTORCRAFT_TUNE_HOVER" period=".1"/>
|
||||||
|
<!-- <message name="GPS_INT" period=".20"/> -->
|
||||||
|
<!--<message name="INS2" period=".05"/>
|
||||||
|
<message name="INS3" period=".20"/>-->
|
||||||
|
<message name="INS_REF" period="5.1"/>
|
||||||
|
</mode>
|
||||||
|
|
||||||
|
<mode name="empty"></mode>
|
||||||
|
|
||||||
|
</process>
|
||||||
|
<process name="Logger">
|
||||||
|
<mode name="default">
|
||||||
|
<message name="ROTORCRAFT_STATUS" period="1.2"/>
|
||||||
|
<message name="DL_VALUE" period="0.5"/>
|
||||||
|
<message name="ALIVE" period="2.1"/>
|
||||||
|
<message name="IMU_GYRO_SCALED" period=".002"/>
|
||||||
|
<message name="IMU_ACCEL_SCALED" period=".002"/>
|
||||||
|
<message name="IMU_MAG_SCALED" period=".1"/>
|
||||||
|
</mode>
|
||||||
|
</process>
|
||||||
|
|
||||||
|
</telemetry>
|
||||||
|
|
||||||
@@ -0,0 +1,396 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) Bart Slinger
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/**
|
||||||
|
* @file "modules/loggers/sdlogger_spi_direct.c"
|
||||||
|
* @author Bart Slinger
|
||||||
|
* SPI SD Logger that saves pprzlog messages to SD Card.
|
||||||
|
*
|
||||||
|
* Developed using Test Driven Development.
|
||||||
|
* Test code available at:
|
||||||
|
* https://github.com/bartslinger/paparazzi-unittest
|
||||||
|
*/
|
||||||
|
|
||||||
|
#define PERIODIC_C_LOGGER
|
||||||
|
|
||||||
|
#include "modules/loggers/sdlogger_spi_direct.h"
|
||||||
|
|
||||||
|
#ifdef LOGGER_LED
|
||||||
|
#define LOGGER_LED_ON LED_ON(LOGGER_LED);
|
||||||
|
#define LOGGER_LED_OFF LED_OFF(LOGGER_LED);
|
||||||
|
#else
|
||||||
|
#define LOGGER_LED_ON {}
|
||||||
|
#define LOGGER_LED_OFF {}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef TELEMETRY_MODE_Main_empty
|
||||||
|
#warning You need to define a main telemetry mode named "empty" without any \
|
||||||
|
messages in your config file in /conf/telemetry/<your_config.xml>. \
|
||||||
|
\
|
||||||
|
Add <mode name="empty"></mode> to your main telemetry process.
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef TELEMETRY_PROCESS_Logger
|
||||||
|
#error "You need to use a telemetry xml file with Logger process!"
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef DOWNLINK_DEVICE
|
||||||
|
#warning This module can only be used with uart downlink for now.
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct sdlogger_spi_periph sdlogger_spi;
|
||||||
|
|
||||||
|
/* Private function declarations */
|
||||||
|
void sdlogger_spi_direct_block_to_uart(void);
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief sdlogger_spi_direct_init
|
||||||
|
* Initialize the logger and SD Card.
|
||||||
|
*/
|
||||||
|
void sdlogger_spi_direct_init(void)
|
||||||
|
{
|
||||||
|
/* Initialize the SD Card */
|
||||||
|
sdcard_spi_init(&sdcard1, &(SDLOGGER_SPI_LINK_DEVICE),
|
||||||
|
SDLOGGER_SPI_LINK_SLAVE_NUMBER);
|
||||||
|
|
||||||
|
/* Set values in the struct to their defaults */
|
||||||
|
sdlogger_spi.status = SDLogger_Initializing;
|
||||||
|
sdlogger_spi.next_available_address = 0;
|
||||||
|
sdlogger_spi.last_completed = 0;
|
||||||
|
sdlogger_spi.sdcard_buf_idx = 1;
|
||||||
|
|
||||||
|
/* Fill internal buffer with zeros */
|
||||||
|
for (uint8_t i = 0; i < sizeof(sdlogger_spi.buffer); i++) {
|
||||||
|
sdlogger_spi.buffer[i] = 0;
|
||||||
|
}
|
||||||
|
sdlogger_spi.idx = 0;
|
||||||
|
sdlogger_spi.log_len = 0;
|
||||||
|
sdlogger_spi.command = 0;
|
||||||
|
sdlogger_spi.download_id = 0;
|
||||||
|
sdlogger_spi.download_address = 0;
|
||||||
|
sdlogger_spi.download_length = 0;
|
||||||
|
|
||||||
|
/* Set function pointers in link_device to the logger functions */
|
||||||
|
sdlogger_spi.device.check_free_space = (check_free_space_t)sdlogger_spi_direct_check_free_space;
|
||||||
|
sdlogger_spi.device.put_byte = (put_byte_t)sdlogger_spi_direct_put_byte;
|
||||||
|
sdlogger_spi.device.send_message = (send_message_t)sdlogger_spi_direct_send_message;
|
||||||
|
sdlogger_spi.device.char_available = (char_available_t)sdlogger_spi_direct_char_available;
|
||||||
|
sdlogger_spi.device.get_byte = (get_byte_t)sdlogger_spi_direct_get_byte;
|
||||||
|
sdlogger_spi.device.periph = &sdlogger_spi;
|
||||||
|
|
||||||
|
/* Init pprzlog_tp */
|
||||||
|
pprzlog_transport_init();
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief sdlogger_spi_direct_periodic
|
||||||
|
* Periodic function called at module frequency
|
||||||
|
*/
|
||||||
|
void sdlogger_spi_direct_periodic(void)
|
||||||
|
{
|
||||||
|
sdcard_spi_periodic(&sdcard1);
|
||||||
|
|
||||||
|
switch (sdlogger_spi.status) {
|
||||||
|
case SDLogger_Initializing:
|
||||||
|
if (sdcard1.status == SDCard_Idle) {
|
||||||
|
sdcard_spi_read_block(&sdcard1, 0x00002000, &sdlogger_spi_direct_index_received);
|
||||||
|
sdlogger_spi.status = SDLogger_RetreivingIndex;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_Ready:
|
||||||
|
if (radio_control.values[SDLOGGER_CONTROL_SWITCH] > 0 &&
|
||||||
|
sdcard1.status == SDCard_Idle) {
|
||||||
|
LOGGER_LED_ON;
|
||||||
|
sdcard_spi_multiwrite_start(&sdcard1, sdlogger_spi.next_available_address);
|
||||||
|
sdlogger_spi.status = SDLogger_Logging;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_Logging:
|
||||||
|
/* This line is NOT unit-tested because it is an inline function */
|
||||||
|
#if PERIODIC_TELEMETRY
|
||||||
|
periodic_telemetry_send_Logger(DefaultPeriodic,
|
||||||
|
&pprzlog_tp.trans_tx,
|
||||||
|
&sdlogger_spi.device);
|
||||||
|
#endif
|
||||||
|
/* Check if SD Card buffer is full and SD Card is ready for new data */
|
||||||
|
if (sdlogger_spi.sdcard_buf_idx > 512 &&
|
||||||
|
sdcard1.status == SDCard_MultiWriteIdle) {
|
||||||
|
sdcard_spi_multiwrite_next(&sdcard1, &sdlogger_spi_direct_multiwrite_written);
|
||||||
|
}
|
||||||
|
/* Check if switch is flipped to stop logging */
|
||||||
|
if (radio_control.values[SDLOGGER_CONTROL_SWITCH] < 0) {
|
||||||
|
sdlogger_spi.status = SDLogger_LoggingFinalBlock;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_LoggingFinalBlock:
|
||||||
|
if (sdcard1.status == SDCard_MultiWriteIdle) {
|
||||||
|
if (sdlogger_spi.sdcard_buf_idx > 512) {
|
||||||
|
sdcard_spi_multiwrite_next(&sdcard1, &sdlogger_spi_direct_multiwrite_written);
|
||||||
|
}
|
||||||
|
else if (sdlogger_spi.sdcard_buf_idx > 1) {
|
||||||
|
/* Fill with trailing zero's */
|
||||||
|
for (uint16_t i = sdlogger_spi.sdcard_buf_idx; i < (SD_BLOCK_SIZE+1); i++) {
|
||||||
|
sdcard1.output_buf[i] = 0x00;
|
||||||
|
}
|
||||||
|
sdcard_spi_multiwrite_next(&sdcard1, &sdlogger_spi_direct_multiwrite_written);
|
||||||
|
}
|
||||||
|
else if (sdlogger_spi.sdcard_buf_idx == 1) {
|
||||||
|
sdcard_spi_multiwrite_stop(&sdcard1);
|
||||||
|
sdlogger_spi.status = SDLogger_StoppedLogging;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_StoppedLogging:
|
||||||
|
if (sdcard1.status == SDCard_Idle) {
|
||||||
|
sdcard_spi_read_block(&sdcard1, 0x00002000, &sdlogger_spi_direct_index_received);
|
||||||
|
sdlogger_spi.status = SDLogger_GettingIndexForUpdate;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_UpdatingIndex:
|
||||||
|
if (sdcard1.status == SDCard_Idle) {
|
||||||
|
LOGGER_LED_OFF;
|
||||||
|
sdlogger_spi.status = SDLogger_Ready;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_Downloading:
|
||||||
|
if (sdcard1.status == SDCard_Idle) {
|
||||||
|
/* Put bytes to the buffer until all is written or buffer is full */
|
||||||
|
for (uint16_t i = sdlogger_spi.sdcard_buf_idx; i < SD_BLOCK_SIZE; i++) {
|
||||||
|
if(uart_check_free_space(&(DOWNLINK_DEVICE), 1)) {
|
||||||
|
uart_put_byte(&(DOWNLINK_DEVICE), sdcard1.input_buf[i]);
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
/* No free space left, abort for-loop */
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
sdlogger_spi.sdcard_buf_idx++;
|
||||||
|
}
|
||||||
|
/* Request next block if entire buffer was written to uart */
|
||||||
|
if (sdlogger_spi.sdcard_buf_idx >= SD_BLOCK_SIZE) {
|
||||||
|
if (sdlogger_spi.download_length > 0) {
|
||||||
|
sdcard_spi_read_block(&sdcard1, sdlogger_spi.download_address, NULL);
|
||||||
|
sdlogger_spi.download_address++;
|
||||||
|
sdlogger_spi.download_length--;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
LOGGER_LED_OFF;
|
||||||
|
sdlogger_spi.status = SDLogger_Ready;
|
||||||
|
}
|
||||||
|
sdlogger_spi.sdcard_buf_idx = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void sdlogger_spi_direct_start(void) {}
|
||||||
|
void sdlogger_spi_direct_stop(void) {}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief sdlogger_spi_direct_index_received
|
||||||
|
* Callback from SD Card when block at index location is received.
|
||||||
|
*/
|
||||||
|
void sdlogger_spi_direct_index_received(void)
|
||||||
|
{
|
||||||
|
|
||||||
|
switch (sdlogger_spi.status) {
|
||||||
|
case SDLogger_RetreivingIndex:
|
||||||
|
sdlogger_spi.next_available_address = 0x00004000;
|
||||||
|
sdlogger_spi.last_completed = 0;
|
||||||
|
/* Save data for later use
|
||||||
|
sdlogger_spi.next_available_address = (sdcard1.input_buf[0] << 24) |
|
||||||
|
(sdcard1.input_buf[1] << 16) |
|
||||||
|
(sdcard1.input_buf[2] << 8) |
|
||||||
|
(sdcard1.input_buf[3]);
|
||||||
|
sdlogger_spi.last_completed = sdcard1.input_buf[4];
|
||||||
|
*/
|
||||||
|
|
||||||
|
/* Ready to start logging */
|
||||||
|
sdlogger_spi.status = SDLogger_Ready;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_GettingIndexForUpdate:
|
||||||
|
/* Copy input buffer to output buffer */
|
||||||
|
for (uint16_t i = 0; i < SD_BLOCK_SIZE; i++) {
|
||||||
|
sdcard1.output_buf[i+6] = sdcard1.input_buf[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Increment last completed log */
|
||||||
|
sdcard1.output_buf[4+6] = ++sdlogger_spi.last_completed;
|
||||||
|
/* Write log info at dedicated location */
|
||||||
|
{
|
||||||
|
uint16_t log_idx_start = 5 + 6 + (sdlogger_spi.last_completed - 1) * 12;
|
||||||
|
|
||||||
|
/* Set start address and length at location that belongs to the log nr */
|
||||||
|
sdcard1.output_buf[log_idx_start+0] = sdlogger_spi.next_available_address >> 24;
|
||||||
|
sdcard1.output_buf[log_idx_start+1] = sdlogger_spi.next_available_address >> 16;
|
||||||
|
sdcard1.output_buf[log_idx_start+2] = sdlogger_spi.next_available_address >> 8;
|
||||||
|
sdcard1.output_buf[log_idx_start+3] = sdlogger_spi.next_available_address >> 0;
|
||||||
|
sdcard1.output_buf[log_idx_start+4] = sdlogger_spi.log_len >> 24;
|
||||||
|
sdcard1.output_buf[log_idx_start+5] = sdlogger_spi.log_len >> 16;
|
||||||
|
sdcard1.output_buf[log_idx_start+6] = sdlogger_spi.log_len >> 8;
|
||||||
|
sdcard1.output_buf[log_idx_start+7] = sdlogger_spi.log_len >> 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* Increment and update the next available address */
|
||||||
|
sdlogger_spi.next_available_address += sdlogger_spi.log_len;
|
||||||
|
sdcard1.output_buf[0+6] = sdlogger_spi.next_available_address >> 24;
|
||||||
|
sdcard1.output_buf[1+6] = sdlogger_spi.next_available_address >> 16;
|
||||||
|
sdcard1.output_buf[2+6] = sdlogger_spi.next_available_address >> 8;
|
||||||
|
sdcard1.output_buf[3+6] = sdlogger_spi.next_available_address >> 0;
|
||||||
|
|
||||||
|
sdcard_spi_write_block(&sdcard1, 0x00002000);
|
||||||
|
/* Reset log length */
|
||||||
|
sdlogger_spi.log_len = 0;
|
||||||
|
sdlogger_spi.status = SDLogger_UpdatingIndex;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case SDLogger_GettingIndexForDownload:
|
||||||
|
{
|
||||||
|
uint16_t info_idx = 5 + (sdlogger_spi.download_id - 1) * 12;
|
||||||
|
sdlogger_spi.download_address = (sdcard1.input_buf[info_idx+0] << 24) |
|
||||||
|
(sdcard1.input_buf[info_idx+1] << 16) |
|
||||||
|
(sdcard1.input_buf[info_idx+2] << 8) |
|
||||||
|
(sdcard1.input_buf[info_idx+3] << 0);
|
||||||
|
sdlogger_spi.download_length = (sdcard1.input_buf[info_idx+4] << 24) |
|
||||||
|
(sdcard1.input_buf[info_idx+5] << 16) |
|
||||||
|
(sdcard1.input_buf[info_idx+6] << 8) |
|
||||||
|
(sdcard1.input_buf[info_idx+7] << 0);
|
||||||
|
if (sdlogger_spi.download_length > 0) {
|
||||||
|
/* Request the first block */
|
||||||
|
sdcard_spi_read_block(&sdcard1, sdlogger_spi.download_address, NULL);
|
||||||
|
/* After each read block, incr address, decr length */
|
||||||
|
sdlogger_spi.download_address++;
|
||||||
|
sdlogger_spi.download_length--;
|
||||||
|
sdlogger_spi.status = SDLogger_Downloading;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
LOGGER_LED_OFF;
|
||||||
|
sdlogger_spi.status = SDLogger_Ready;
|
||||||
|
}
|
||||||
|
sdlogger_spi.sdcard_buf_idx = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @brief sdlogger_spi_direct_multiwrite_written
|
||||||
|
* Called when a multiwrite is complete. Data stored in the logger buffer is
|
||||||
|
* then moved to the SD Card buffer, which is now available again.
|
||||||
|
*/
|
||||||
|
void sdlogger_spi_direct_multiwrite_written(void)
|
||||||
|
{
|
||||||
|
/* Increment log length */
|
||||||
|
sdlogger_spi.log_len++;
|
||||||
|
|
||||||
|
/* Copy data from logger buffer to SD Card buffer */
|
||||||
|
for (uint8_t i = 0; i < sdlogger_spi.idx; i++) {
|
||||||
|
sdcard1.output_buf[i+1] = sdlogger_spi.buffer[i];
|
||||||
|
}
|
||||||
|
/* Set sdcard buffer index to new value */
|
||||||
|
sdlogger_spi.sdcard_buf_idx = sdlogger_spi.idx + 1;
|
||||||
|
/* And reset the logger buffer index */
|
||||||
|
sdlogger_spi.idx = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void sdlogger_spi_direct_command(void)
|
||||||
|
{
|
||||||
|
if (sdcard1.status == SDCard_Idle && sdlogger_spi.command > 0 &&
|
||||||
|
sdlogger_spi.command < 43) {
|
||||||
|
LOGGER_LED_ON;
|
||||||
|
sdcard_spi_read_block(&sdcard1, 0x00002000,
|
||||||
|
&sdlogger_spi_direct_index_received);
|
||||||
|
sdlogger_spi.download_id = sdlogger_spi.command;
|
||||||
|
sdlogger_spi.status = SDLogger_GettingIndexForDownload;
|
||||||
|
}
|
||||||
|
else if (sdcard1.status == SDCard_Idle && sdlogger_spi.command == 255) {
|
||||||
|
telemetry_mode_Main = TELEMETRY_MODE_Main_empty;
|
||||||
|
LOGGER_LED_ON;
|
||||||
|
sdcard_spi_read_block(&sdcard1, 0x00002000, NULL);
|
||||||
|
sdlogger_spi.download_length = 0;
|
||||||
|
sdlogger_spi.sdcard_buf_idx = 0;
|
||||||
|
sdlogger_spi.status = SDLogger_Downloading;
|
||||||
|
}
|
||||||
|
/* Always reset command value back to zero */
|
||||||
|
sdlogger_spi.command = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool_t sdlogger_spi_direct_check_free_space(struct sdlogger_spi_periph *p, uint8_t len)
|
||||||
|
{
|
||||||
|
if (p->status == SDLogger_Logging) {
|
||||||
|
/* Calculating free space in both buffers */
|
||||||
|
if ( (513 - p->sdcard_buf_idx) + (SDLOGGER_BUFFER_SIZE - p->idx) >= len) {
|
||||||
|
return TRUE;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return FALSE;
|
||||||
|
}
|
||||||
|
|
||||||
|
void sdlogger_spi_direct_put_byte(struct sdlogger_spi_periph *p, uint8_t data)
|
||||||
|
{
|
||||||
|
/* SD Buffer full, write in logger buffer */
|
||||||
|
if (p->sdcard_buf_idx > 512) {
|
||||||
|
if (p->idx < SDLOGGER_BUFFER_SIZE) {
|
||||||
|
p->buffer[p->idx++] = data;
|
||||||
|
}
|
||||||
|
/* else: data lost */
|
||||||
|
}
|
||||||
|
/* Writing directly to SD Card buffer */
|
||||||
|
else {
|
||||||
|
sdcard1.output_buf[p->sdcard_buf_idx++] = data;
|
||||||
|
|
||||||
|
/* Flush buffer */
|
||||||
|
if (p->sdcard_buf_idx > 512 && sdcard1.status == SDCard_MultiWriteIdle) {
|
||||||
|
sdcard_spi_multiwrite_next(&sdcard1, &sdlogger_spi_direct_multiwrite_written);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void sdlogger_spi_direct_send_message(void *p)
|
||||||
|
{
|
||||||
|
(void) p;
|
||||||
|
}
|
||||||
|
|
||||||
|
int sdlogger_spi_direct_char_available(void *p){
|
||||||
|
(void) p;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t sdlogger_spi_direct_get_byte(void *p)
|
||||||
|
{
|
||||||
|
(void) p;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,90 @@
|
|||||||
|
/*
|
||||||
|
* Copyright (C) Bart Slinger
|
||||||
|
*
|
||||||
|
* This file is part of paparazzi
|
||||||
|
*
|
||||||
|
* paparazzi is free software; you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation; either version 2, or (at your option)
|
||||||
|
* any later version.
|
||||||
|
*
|
||||||
|
* paparazzi is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with paparazzi; see the file COPYING. If not, see
|
||||||
|
* <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
/**
|
||||||
|
* @file "modules/loggers/sdlogger_spi_direct.h"
|
||||||
|
* @author Bart Slinger
|
||||||
|
* SPI SD Logger that saves pprzlog messages to SD Card.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef SDLOGGER_SPI_H
|
||||||
|
#define SDLOGGER_SPI_H
|
||||||
|
|
||||||
|
#define SDLOGGER_BUFFER_SIZE 128
|
||||||
|
|
||||||
|
#include "std.h"
|
||||||
|
#include "mcu_periph/link_device.h"
|
||||||
|
#include "subsystems/radio_control.h"
|
||||||
|
#include "subsystems/datalink/pprzlog_transport.h"
|
||||||
|
#include "subsystems/datalink/telemetry.h"
|
||||||
|
#include "peripherals/sdcard_spi.h"
|
||||||
|
#include "mcu_periph/uart.h"
|
||||||
|
#include "led.h"
|
||||||
|
|
||||||
|
#include "generated/periodic_telemetry.h"
|
||||||
|
|
||||||
|
enum SDLoggerStatus {
|
||||||
|
SDLogger_UnInit,
|
||||||
|
SDLogger_Error,
|
||||||
|
SDLogger_Initializing,
|
||||||
|
SDLogger_RetreivingIndex,
|
||||||
|
SDLogger_Ready,
|
||||||
|
SDLogger_Logging,
|
||||||
|
SDLogger_LoggingFinalBlock,
|
||||||
|
SDLogger_StoppedLogging,
|
||||||
|
SDLogger_GettingIndexForUpdate,
|
||||||
|
SDLogger_UpdatingIndex,
|
||||||
|
SDLogger_GettingIndexForDownload,
|
||||||
|
SDLogger_Downloading
|
||||||
|
};
|
||||||
|
|
||||||
|
struct sdlogger_spi_periph{
|
||||||
|
enum SDLoggerStatus status;
|
||||||
|
uint32_t next_available_address;
|
||||||
|
uint8_t last_completed;
|
||||||
|
uint16_t sdcard_buf_idx;
|
||||||
|
uint8_t buffer[SDLOGGER_BUFFER_SIZE];
|
||||||
|
uint8_t idx;
|
||||||
|
uint32_t log_len;
|
||||||
|
uint8_t command;
|
||||||
|
uint8_t download_id;
|
||||||
|
uint32_t download_address;
|
||||||
|
uint32_t download_length;
|
||||||
|
struct link_device device;
|
||||||
|
};
|
||||||
|
|
||||||
|
extern struct sdlogger_spi_periph sdlogger_spi;
|
||||||
|
|
||||||
|
extern void sdlogger_spi_direct_init(void);
|
||||||
|
extern void sdlogger_spi_direct_periodic(void);
|
||||||
|
extern void sdlogger_spi_direct_start(void);
|
||||||
|
extern void sdlogger_spi_direct_stop(void);
|
||||||
|
|
||||||
|
extern void sdlogger_spi_direct_index_received(void);
|
||||||
|
extern void sdlogger_spi_direct_multiwrite_written(void);
|
||||||
|
extern void sdlogger_spi_direct_command(void);
|
||||||
|
|
||||||
|
extern bool_t sdlogger_spi_direct_check_free_space(struct sdlogger_spi_periph *p, uint8_t len);
|
||||||
|
extern void sdlogger_spi_direct_put_byte(struct sdlogger_spi_periph *p, uint8_t data);
|
||||||
|
extern void sdlogger_spi_direct_send_message(void *p);
|
||||||
|
extern int sdlogger_spi_direct_char_available(void *p);
|
||||||
|
extern uint8_t sdlogger_spi_direct_get_byte(void *p);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
@@ -35,6 +35,11 @@
|
|||||||
* The following resource was used as implementation reference: http://elm-chan.org/docs/mmc/mmc_e.html
|
* The following resource was used as implementation reference: http://elm-chan.org/docs/mmc/mmc_e.html
|
||||||
* The initialization procedure is implemented according to the following diagram. Only the branches for SD ver.2 are currently included.
|
* The initialization procedure is implemented according to the following diagram. Only the branches for SD ver.2 are currently included.
|
||||||
* \image html airborne/sdinit.png
|
* \image html airborne/sdinit.png
|
||||||
|
*
|
||||||
|
* Developed using Test Driven Development.
|
||||||
|
* Test code available at:
|
||||||
|
* https://github.com/bartslinger/paparazzi-unittest
|
||||||
|
*
|
||||||
* @todo CRC checksums are not implemented. Fake values of 0xFF are used and they are ignored by the card.
|
* @todo CRC checksums are not implemented. Fake values of 0xFF are used and they are ignored by the card.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
@@ -380,8 +385,8 @@ void sdcard_spi_spicallback(struct spi_transaction *t)
|
|||||||
/* Data block received in buffer, process data */
|
/* Data block received in buffer, process data */
|
||||||
case SDCard_ReadingDataBlock:
|
case SDCard_ReadingDataBlock:
|
||||||
sdcard1.status = SDCard_Idle;
|
sdcard1.status = SDCard_Idle;
|
||||||
if (sdcard1.read_callback != NULL) {
|
if (sdcard1.external_callback != NULL) {
|
||||||
sdcard1.read_callback();
|
sdcard1.external_callback();
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@@ -408,6 +413,9 @@ void sdcard_spi_spicallback(struct spi_transaction *t)
|
|||||||
case SDCard_MultiWriteWriting:
|
case SDCard_MultiWriteWriting:
|
||||||
if ((sdcard1.input_buf[SD_BLOCK_SIZE + 3] & 0x0F) == 0x05 /* Data accepted */) {
|
if ((sdcard1.input_buf[SD_BLOCK_SIZE + 3] & 0x0F) == 0x05 /* Data accepted */) {
|
||||||
sdcard1.status = SDCard_MultiWriteBusy;
|
sdcard1.status = SDCard_MultiWriteBusy;
|
||||||
|
if(sdcard1.external_callback != NULL) {
|
||||||
|
sdcard1.external_callback();
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
sdcard1.status = SDCard_Error;
|
sdcard1.status = SDCard_Error;
|
||||||
}
|
}
|
||||||
@@ -576,7 +584,7 @@ void sdcard_spi_read_block(struct SDCard *sdcard, uint32_t addr, SDCardCallback
|
|||||||
}
|
}
|
||||||
|
|
||||||
/* Set function to be called after the read action has finished. */
|
/* Set function to be called after the read action has finished. */
|
||||||
sdcard->read_callback = callback;
|
sdcard->external_callback = callback;
|
||||||
|
|
||||||
/* Send command 17 (read block) to the SDCard */
|
/* Send command 17 (read block) to the SDCard */
|
||||||
sdcard_spi_send_cmd(sdcard, 17, addr);
|
sdcard_spi_send_cmd(sdcard, 17, addr);
|
||||||
@@ -611,7 +619,7 @@ void sdcard_spi_multiwrite_start(struct SDCard *sdcard, uint32_t addr)
|
|||||||
* Use only after sdcard_spi_multiwrite_start().
|
* Use only after sdcard_spi_multiwrite_start().
|
||||||
* @param sdcard Pointer to the SDCard.
|
* @param sdcard Pointer to the SDCard.
|
||||||
*/
|
*/
|
||||||
void sdcard_spi_multiwrite_next(struct SDCard *sdcard)
|
void sdcard_spi_multiwrite_next(struct SDCard *sdcard, SDCardCallback callback)
|
||||||
{
|
{
|
||||||
/* Can only write next block if card is in multiwrite mode and not currently busy */
|
/* Can only write next block if card is in multiwrite mode and not currently busy */
|
||||||
if (sdcard->status != SDCard_MultiWriteIdle) {
|
if (sdcard->status != SDCard_MultiWriteIdle) {
|
||||||
@@ -627,6 +635,7 @@ void sdcard_spi_multiwrite_next(struct SDCard *sdcard)
|
|||||||
|
|
||||||
/* Set the callback */
|
/* Set the callback */
|
||||||
sdcard->spi_t.after_cb = &sdcard_spi_spicallback;
|
sdcard->spi_t.after_cb = &sdcard_spi_spicallback;
|
||||||
|
sdcard->external_callback = callback;
|
||||||
|
|
||||||
/* Submit the spi transaction */
|
/* Submit the spi transaction */
|
||||||
spi_submit(sdcard->spi_p, &sdcard->spi_t);
|
spi_submit(sdcard->spi_p, &sdcard->spi_t);
|
||||||
|
|||||||
@@ -96,7 +96,7 @@ struct SDCard {
|
|||||||
uint8_t response_counter; /**< Response counter used at various locations */
|
uint8_t response_counter; /**< Response counter used at various locations */
|
||||||
uint32_t timeout_counter; /**< Timeout counter used for initialization checks with ACMD41 */
|
uint32_t timeout_counter; /**< Timeout counter used for initialization checks with ACMD41 */
|
||||||
enum SDCardType card_type; /**< Type of SDCard */
|
enum SDCardType card_type; /**< Type of SDCard */
|
||||||
SDCardCallback read_callback; /**< Callback to call when read operation finishes */
|
SDCardCallback external_callback; /**< Callback to call when external operation finishes */
|
||||||
};
|
};
|
||||||
|
|
||||||
extern struct SDCard sdcard1;
|
extern struct SDCard sdcard1;
|
||||||
@@ -107,7 +107,7 @@ extern void sdcard_spi_periodic(struct SDCard *sdcard);
|
|||||||
extern void sdcard_spi_write_block(struct SDCard *sdcard, uint32_t addr);
|
extern void sdcard_spi_write_block(struct SDCard *sdcard, uint32_t addr);
|
||||||
extern void sdcard_spi_read_block(struct SDCard *sdcard, uint32_t addr, SDCardCallback callback);
|
extern void sdcard_spi_read_block(struct SDCard *sdcard, uint32_t addr, SDCardCallback callback);
|
||||||
extern void sdcard_spi_multiwrite_start(struct SDCard *sdcard, uint32_t addr);
|
extern void sdcard_spi_multiwrite_start(struct SDCard *sdcard, uint32_t addr);
|
||||||
extern void sdcard_spi_multiwrite_next(struct SDCard *sdcard);
|
extern void sdcard_spi_multiwrite_next(struct SDCard *sdcard, SDCardCallback callback);
|
||||||
extern void sdcard_spi_multiwrite_stop(struct SDCard *sdcard);
|
extern void sdcard_spi_multiwrite_stop(struct SDCard *sdcard);
|
||||||
|
|
||||||
#endif // SDCARD_H_
|
#endif // SDCARD_H_
|
||||||
|
|||||||
@@ -30,7 +30,7 @@ LINKPKG = $(PKG) -linkpkg -dllpath-pkg pprz
|
|||||||
XPKG = -package pprz.xlib
|
XPKG = -package pprz.xlib
|
||||||
XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib
|
XLINKPKG = $(XPKG) -linkpkg -dllpath-pkg pprz.xlib
|
||||||
|
|
||||||
all: play plotter logplotter sd2log plotprofile openlog2tlm
|
all: play plotter logplotter sd2log plotprofile openlog2tlm sdlogger_download
|
||||||
|
|
||||||
play : log_file.cmo play_core.cmo play.cmo $(LIBPPRZCMA)
|
play : log_file.cmo play_core.cmo play.cmo $(LIBPPRZCMA)
|
||||||
@echo OL $@
|
@echo OL $@
|
||||||
@@ -52,6 +52,9 @@ sd2log : sd2log.cmo $(LIBPPRZCMA)
|
|||||||
@echo OL $@
|
@echo OL $@
|
||||||
$(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^
|
$(Q)$(OCAMLC) $(INCLUDES) -o $@ $(LINKPKG) $^
|
||||||
|
|
||||||
|
sdlogger_download: sdlogger_download.c
|
||||||
|
@echo CC $@
|
||||||
|
$(Q)$(CC) $(CFLAGS) -std=gnu99 -Wno-unused-result -o $@ $^
|
||||||
|
|
||||||
# Target for bytecode executable (if ocamlopt is not available)
|
# Target for bytecode executable (if ocamlopt is not available)
|
||||||
# plot : log_file.cmo gtk_export.cmo export.cmo plot.cmo
|
# plot : log_file.cmo gtk_export.cmo export.cmo plot.cmo
|
||||||
@@ -135,7 +138,7 @@ ctrlstick: ctrlstick.c
|
|||||||
|
|
||||||
|
|
||||||
clean:
|
clean:
|
||||||
$(Q)rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrs2fg logplotter plotter gtk_export.ml openlog2tlm disp3d plotprofile tmclient ffjoystick ctrlstick sd2log
|
$(Q)rm -f *.opt *.out *~ core *.o *.bak .depend *.cm* play ahrs2fg logplotter plotter gtk_export.ml openlog2tlm disp3d plotprofile tmclient ffjoystick ctrlstick sd2log sdlogger_download
|
||||||
|
|
||||||
.PHONY: all clean
|
.PHONY: all clean
|
||||||
|
|
||||||
|
|||||||
File diff suppressed because it is too large
Load Diff
Executable
+15
@@ -0,0 +1,15 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
import sys
|
||||||
|
from os import path, getenv
|
||||||
|
|
||||||
|
# if PAPARAZZI_SRC not set, then assume the tree containing this
|
||||||
|
# file is a reasonable substitute
|
||||||
|
PPRZ_SRC = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
|
||||||
|
sys.path.append(PPRZ_SRC + "/sw/lib/python")
|
||||||
|
from settings_xml_parse import PaparazziACSettings
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
ac_id = int(sys.argv[1])
|
||||||
|
command_name = sys.argv[2]
|
||||||
|
settings = PaparazziACSettings(ac_id)
|
||||||
|
print settings.name_lookup[command_name].index
|
||||||
Reference in New Issue
Block a user