--- /dev/null
+/*
+ * Copyright Droids Corporation (2010)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_commands.h,v 1.9 2009-05-27 20:04:06 zer0 Exp $
+ *
+ */
+
+#ifndef _I2C_COMMANDS_H_
+#define _I2C_COMMANDS_H_
+
+#define I2C_MAINBOARD_ADDR 1
+#define I2C_IMUBOARD_ADDR 2
+
+struct i2c_cmd_hdr {
+ uint8_t cmd;
+};
+
+/****/
+/* commands that do not need and answer */
+/****/
+
+#define I2C_CMD_GENERIC_LED_CONTROL 0x00
+
+struct i2c_cmd_led_control {
+ struct i2c_cmd_hdr hdr;
+ uint8_t led_num:7;
+ uint8_t state:1;
+};
+
+/****/
+/* requests and their answers */
+/****/
+
+
+#define I2C_REQ_IMUBOARD_STATUS 0x80
+
+struct i2c_req_imuboard_status {
+ struct i2c_cmd_hdr hdr;
+ /* add data in request if any */
+};
+
+#define I2C_ANS_IMUBOARD_STATUS 0x81
+
+struct i2c_ans_imuboard_status {
+ struct i2c_cmd_hdr hdr;
+
+#define IMUBOARD_STATUS_GPS_OK 0x01
+#define IMUBOARD_STATUS_IMU_OK 0x02
+ uint8_t flags;
+
+ int32_t latitude; /* between -90e7 and 90e7, in 1/1e-7 degrees,
+ * positive means north hemisphere */
+ int32_t longitude; /* between -180e7 and 180e7, in 1/1e-7 degrees,
+ * positive means east */
+ uint32_t altitude; /* altitude from elipsoid, in 1/100 meters */
+
+ uint16_t roll; /* XXX unit ? */
+ uint16_t pitch;
+ uint16_t yaw;
+};
+
+#endif /* _I2C_PROTOCOL_H_ */
--- /dev/null
+- printf -> printf_P
+- check log rate limitation
+- eeprom check to avoid programming the mcu with the prog of another board
--- /dev/null
+# Test for Razor 9DOF IMU\r
+# Jose Julio @2009\r
+# This script needs VPhyton, pyserial and pywin modules\r
+\r
+# First Install Python 2.6.4\r
+# Install pywin from http://sourceforge.net/projects/pywin32/\r
+# Install pyserial from http://sourceforge.net/projects/pyserial/files/\r
+# Install Vphyton from http://vpython.org/contents/download_windows.html\r
+\r
+from visual import *\r
+import serial\r
+import string\r
+import math\r
+import sys\r
+\r
+from time import time\r
+\r
+#ev = scene.waitfor('click keydown')\r
+\r
+grad2rad = 3.141592/180.0\r
+\r
+# Check your COM port and baud rate\r
+ser = serial.Serial(port='/dev/ttyUSB0',baudrate=57600, timeout=1)\r
+\r
+# Main scene\r
+scene=display(title="9DOF Razor IMU test")\r
+scene.range=(1.2,1.2,1.2)\r
+#scene.forward = (0,-1,-0.25)\r
+scene.forward = (1,0,-0.25)\r
+scene.up=(0,0,1)\r
+\r
+# Second scene (Roll, Pitch, Yaw)\r
+scene2 = display(title='9DOF Razor IMU test',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0))\r
+scene2.range=(1,1,1)\r
+scene.width=500\r
+scene.y=200\r
+\r
+scene2.select()\r
+#Roll, Pitch, Yaw\r
+cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red)\r
+cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red)\r
+cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green)\r
+cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green)\r
+#cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue)\r
+#cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue)\r
+arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1)\r
+\r
+#Roll,Pitch,Yaw labels\r
+label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0)\r
+label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0)\r
+label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0)\r
+label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow)\r
+label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow)\r
+label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow)\r
+label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow)\r
+label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow)\r
+\r
+L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0)\r
+L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0)\r
+L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0)\r
+\r
+# Main scene objects\r
+scene.select()\r
+# Reference axis (x,y,z)\r
+arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1)\r
+arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1)\r
+arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1)\r
+# labels\r
+label(pos=(0,0,0.8),text="9DOF Razor IMU test",box=0,opacity=0)\r
+label(pos=(1,0,0),text="X",box=0,opacity=0)\r
+label(pos=(0,-1,0),text="Y",box=0,opacity=0)\r
+label(pos=(0,0,-1),text="Z",box=0,opacity=0)\r
+# IMU object\r
+platform = box(length=1, height=0.05, width=1, color=color.red)\r
+p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)\r
+plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)\r
+\r
+\r
+f = open("Serial"+str(time())+".txt", 'w')\r
+\r
+roll=0\r
+pitch=0\r
+yaw=0\r
+cpt = 0\r
+t_start = time()\r
+while 1:\r
+ line = ser.readline()\r
+ line = line.replace("!ANG:","") # Delete "!ANG:"\r
+ #print line\r
+ f.write(line) # Write to the output log file\r
+ words = string.split(line,"\t") # Fields split\r
+ #print words\r
+ print words\r
+ if len(words) != 3:\r
+ continue\r
+\r
+ """\r
+ a = float(words[0])#*grad2rad\r
+ b = float(words[1])#*grad2rad\r
+ c = float(words[2])#*grad2rad\r
+ d = float(words[3])#*grad2rad\r
+\r
+ platform.axis = (1, 1, 1)\r
+ #print a, b, c, d\r
+ platform.rotate(angle=a, axis = (b, c, d))\r
+ continue\r
+ """\r
+ try:\r
+ roll = float(words[0])#*grad2rad\r
+ pitch = float(words[1])#*grad2rad\r
+ yaw = float(words[2])#*grad2rad\r
+ except:\r
+ print "Invalid line"\r
+ continue\r
+\r
+ cpt += 1\r
+ if cpt > 10:\r
+ t = time() - t_start\r
+ t_start = time()\r
+ print "XXX", cpt/t\r
+ cpt = 0\r
+\r
+ axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch))\r
+ up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),sin(roll)*cos(yaw)-cos(roll)*sin(pitch)*sin(yaw),-cos(roll)*cos(pitch))\r
+ platform.axis=axis\r
+ platform.up=up\r
+ platform.length=1.0\r
+ platform.width=0.65\r
+ plat_arrow.axis=axis\r
+ plat_arrow.up=up\r
+ plat_arrow.length=0.8\r
+ p_line.axis=axis\r
+ p_line.up=up\r
+ cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0)\r
+ cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0)\r
+ cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)\r
+ cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)\r
+ arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)\r
+ L1.text = str(float(words[0]))\r
+ L2.text = str(float(words[1]))\r
+ L3.text = str(float(words[2]))\r
+ser.close()\r
+f.close()\r
--- /dev/null
+/*\r
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>\r
+ * Copyright (c) 2011-2012, SOH Madgwick\r
+ *\r
+ * This program is free software: you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation, either version 3 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.\r
+ *\r
+ */\r
+\r
+//============================================================================\r
+// MadgwickAHRS.c\r
+//============================================================================\r
+//\r
+// Implementation of Madgwick's IMU and AHRS algorithms.\r
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms\r
+//\r
+// Date Author Notes\r
+// 29/09/2011 SOH Madgwick Initial release\r
+// 02/10/2011 SOH Madgwick Optimised for reduced CPU load\r
+// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised\r
+//\r
+//============================================================================\r
+\r
+#include "MadgwickAHRS.h"\r
+#include <math.h>\r
+\r
+//#define sampleFreq 512.0f // sample frequency in Hz\r
+//#define sampleFreq 46.0f // sample frequency in Hz\r
+#define sampleFreq 85.0f // sample frequency in Hz\r
+#define betaDef 0.1f // 2 * proportional gain\r
+\r
+volatile float beta = betaDef; // 2 * proportional gain (Kp)\r
+volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame\r
+\r
+\r
+static float invSqrt(float x)\r
+{\r
+ return 1.0f / sqrtf(x);\r
+}\r
+\r
+/* AHRS algorithm update */\r
+void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat)\r
+{\r
+ float recipNorm;\r
+ float s0, s1, s2, s3;\r
+ float qDot1, qDot2, qDot3, qDot4;\r
+ float hx, hy;\r
+ float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz;\r
+ float _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3;\r
+ float q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;\r
+ float mx, my, mz, ax, ay, az;\r
+ float q0 = quat->q0;\r
+ float q1 = quat->q1;\r
+ float q2 = quat->q2;\r
+ float q3 = quat->q3;\r
+\r
+ /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in\r
+ * magnetometer normalisation) */\r
+ if ((imu->mx == 0.0f) && (imu->my == 0.0f) && (imu->mz == 0.0f)) {\r
+ MadgwickAHRSupdateIMU(imu, quat);\r
+ return;\r
+ }\r
+\r
+ /* Rate of change of quaternion from gyroscope */\r
+ qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz);\r
+ qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy);\r
+ qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx);\r
+ qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx);\r
+\r
+ /* Compute feedback only if accelerometer measurement valid (avoids NaN\r
+ * in accelerometer normalisation) */\r
+ if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) {\r
+\r
+ /* Normalise accelerometer measurement */\r
+ recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay +\r
+ imu->az * imu->az);\r
+ ax = imu->ax * recipNorm;\r
+ ay = imu->ay * recipNorm;\r
+ az = imu->az * recipNorm;\r
+\r
+ /* Normalise magnetometer measurement */\r
+ recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my +\r
+ imu->mz * imu->mz);\r
+ mx = imu->mx * recipNorm;\r
+ my = imu->my * recipNorm;\r
+ mz = imu->mz * recipNorm;\r
+\r
+ /* Auxiliary variables to avoid repeated arithmetic */\r
+ _2q0mx = 2.0f * q0 * mx;\r
+ _2q0my = 2.0f * q0 * my;\r
+ _2q0mz = 2.0f * q0 * mz;\r
+ _2q1mx = 2.0f * q1 * mx;\r
+ _2q0 = 2.0f * q0;\r
+ _2q1 = 2.0f * q1;\r
+ _2q2 = 2.0f * q2;\r
+ _2q3 = 2.0f * q3;\r
+ _2q0q2 = 2.0f * q0 * q2;\r
+ _2q2q3 = 2.0f * q2 * q3;\r
+ q0q0 = q0 * q0;\r
+ q0q1 = q0 * q1;\r
+ q0q2 = q0 * q2;\r
+ q0q3 = q0 * q3;\r
+ q1q1 = q1 * q1;\r
+ q1q2 = q1 * q2;\r
+ q1q3 = q1 * q3;\r
+ q2q2 = q2 * q2;\r
+ q2q3 = q2 * q3;\r
+ q3q3 = q3 * q3;\r
+\r
+ /* Reference direction of Earth's magnetic field */\r
+ hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;\r
+ hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;\r
+ _2bx = sqrt(hx * hx + hy * hy);\r
+ _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;\r
+ _4bx = 2.0f * _2bx;\r
+ _4bz = 2.0f * _2bz;\r
+\r
+ /* Gradient decent algorithm corrective step */\r
+ s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+ s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+ s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+ s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);\r
+\r
+ /* normalize step magnitude */\r
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);\r
+ s0 *= recipNorm;\r
+ s1 *= recipNorm;\r
+ s2 *= recipNorm;\r
+ s3 *= recipNorm;\r
+\r
+ /* Apply feedback step */\r
+ qDot1 -= beta * s0;\r
+ qDot2 -= beta * s1;\r
+ qDot3 -= beta * s2;\r
+ qDot4 -= beta * s3;\r
+ }\r
+\r
+ /* Integrate rate of change of quaternion to yield quaternion */\r
+ q0 += qDot1 * (1.0f / sampleFreq);\r
+ q1 += qDot2 * (1.0f / sampleFreq);\r
+ q2 += qDot3 * (1.0f / sampleFreq);\r
+ q3 += qDot4 * (1.0f / sampleFreq);\r
+\r
+ /* Normalise quaternion */\r
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);\r
+ quat->q0 = q0 * recipNorm;\r
+ quat->q1 = q1 * recipNorm;\r
+ quat->q2 = q2 * recipNorm;\r
+ quat->q3 = q3 * recipNorm;\r
+}\r
+\r
+/* IMU algorithm update (does not take magneto in account) */\r
+void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat)\r
+{\r
+ float recipNorm;\r
+ float s0, s1, s2, s3;\r
+ float qDot1, qDot2, qDot3, qDot4;\r
+ float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;\r
+ float ax, ay, az;\r
+ float q0 = quat->q0;\r
+ float q1 = quat->q1;\r
+ float q2 = quat->q2;\r
+ float q3 = quat->q3;\r
+\r
+ /* Rate of change of quaternion from gyroscope */\r
+ qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz);\r
+ qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy);\r
+ qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx);\r
+ qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx);\r
+\r
+\r
+ /* Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) */\r
+ if(!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) {\r
+\r
+ /* Normalise accelerometer measurement */\r
+ recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + imu->az * imu->az);\r
+ ax = imu->ax * recipNorm;\r
+ ay = imu->ay * recipNorm;\r
+ az = imu->az * recipNorm;\r
+\r
+ /* Auxiliary variables to avoid repeated arithmetic */\r
+ _2q0 = 2.0f * q0;\r
+ _2q1 = 2.0f * q1;\r
+ _2q2 = 2.0f * q2;\r
+ _2q3 = 2.0f * q3;\r
+ _4q0 = 4.0f * q0;\r
+ _4q1 = 4.0f * q1;\r
+ _4q2 = 4.0f * q2;\r
+ _8q1 = 8.0f * q1;\r
+ _8q2 = 8.0f * q2;\r
+ q0q0 = q0 * q0;\r
+ q1q1 = q1 * q1;\r
+ q2q2 = q2 * q2;\r
+ q3q3 = q3 * q3;\r
+\r
+ /* Gradient decent algorithm corrective step */\r
+ s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;\r
+ s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;\r
+ s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;\r
+ s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;\r
+ recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); /* normalise step magnitude */\r
+\r
+ s0 *= recipNorm;\r
+ s1 *= recipNorm;\r
+ s2 *= recipNorm;\r
+ s3 *= recipNorm;\r
+\r
+ /* Apply feedback step */\r
+ qDot1 -= beta * s0;\r
+ qDot2 -= beta * s1;\r
+ qDot3 -= beta * s2;\r
+ qDot4 -= beta * s3;\r
+ }\r
+\r
+ /* Integrate rate of change of quaternion to yield quaternion */\r
+ q0 += qDot1 * (1.0f / sampleFreq);\r
+ q1 += qDot2 * (1.0f / sampleFreq);\r
+ q2 += qDot3 * (1.0f / sampleFreq);\r
+ q3 += qDot4 * (1.0f / sampleFreq);\r
+\r
+ /* Normalise quaternion */\r
+ recipNorm = invSqrt(q0 * q0 + q1 * q1 +\r
+ q2 * q2 + q3 * q3);\r
+ quat->q0 = q0 * recipNorm;\r
+ quat->q1 = q1 * recipNorm;\r
+ quat->q2 = q2 * recipNorm;\r
+ quat->q3 = q3 * recipNorm;\r
+}\r
+\r
--- /dev/null
+/*\r
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>\r
+ * Copyright (c) 2011, SOH Madgwick\r
+ *\r
+ * This program is free software: you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation, either version 3 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.\r
+ *\r
+ */\r
+\r
+//=============================================================================\r
+// MadgwickAHRS.h\r
+//=============================================================================\r
+//\r
+// Implementation of Madgwick's IMU and AHRS algorithms.\r
+// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms\r
+//\r
+// Date Author Notes\r
+// 29/09/2011 SOH Madgwick Initial release\r
+// 02/10/2011 SOH Madgwick Optimised for reduced CPU load\r
+//\r
+//=============================================================================\r
+\r
+#ifndef MadgwickAHRS_h\r
+#define MadgwickAHRS_h\r
+\r
+#include "imu.h"\r
+\r
+extern volatile float beta; // algorithm gain\r
+\r
+/* update quaternion structure using the new IMU infos */\r
+void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat);\r
+\r
+/* update quaternion structure using the new IMU infos, without using magneto */\r
+void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat);\r
+\r
+#endif\r
--- /dev/null
+TARGET = main
+
+AVERSIVE_DIR ?= ../..
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC = $(TARGET).c
+SRC += commands.c
+SRC += commands_gen.c
+SRC += cmdline.c
+SRC += eeprom_config.c
+SRC += i2cm_sw.c
+SRC += imu.c
+SRC += mpu6050.c
+SRC += MadgwickAHRS.c
+SRC += byteordering.c
+SRC += fat.c
+SRC += sd_main.c
+SRC += partition.c
+SRC += sd_raw.c
+SRC += gps_venus.c
+SRC += sd_log.c
+SRC += i2c_protocol.c
+
+
+CFLAGS += -W -Wall -Werror
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#include "byteordering.h"
+
+/**
+ * \addtogroup byteordering
+ *
+ * Architecture-dependent handling of byte-ordering.
+ *
+ * @{
+ */
+/**
+ * \file
+ * Byte-order handling implementation (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+#if DOXYGEN || SWAP_NEEDED
+
+/**
+ * \internal
+ * Swaps the bytes of a 16-bit integer.
+ *
+ * \param[in] i A 16-bit integer which to swap.
+ * \returns The swapped 16-bit integer.
+ */
+uint16_t swap16(uint16_t i)
+{
+ return SWAP16(i);
+}
+
+/**
+ * \internal
+ * Swaps the bytes of a 32-bit integer.
+ *
+ * \param[in] i A 32-bit integer which to swap.
+ * \returns The swapped 32-bit integer.
+ */
+uint32_t swap32(uint32_t i)
+{
+ return SWAP32(i);
+}
+
+#endif
+
+/**
+ * Reads a 16-bit integer from memory in little-endian byte order.
+ *
+ * \param[in] p Pointer from where to read the integer.
+ * \returns The 16-bit integer read from memory.
+ */
+uint16_t read16(const uint8_t* p)
+{
+ return (((uint16_t) p[1]) << 8) |
+ (((uint16_t) p[0]) << 0);
+}
+
+/**
+ * Reads a 32-bit integer from memory in little-endian byte order.
+ *
+ * \param[in] p Pointer from where to read the integer.
+ * \returns The 32-bit integer read from memory.
+ */
+uint32_t read32(const uint8_t* p)
+{
+ return (((uint32_t) p[3]) << 24) |
+ (((uint32_t) p[2]) << 16) |
+ (((uint32_t) p[1]) << 8) |
+ (((uint32_t) p[0]) << 0);
+}
+
+/**
+ * Writes a 16-bit integer into memory in little-endian byte order.
+ *
+ * \param[in] p Pointer where to write the integer to.
+ * \param[in] i The 16-bit integer to write.
+ */
+void write16(uint8_t* p, uint16_t i)
+{
+ p[1] = (uint8_t) ((i & 0xff00) >> 8);
+ p[0] = (uint8_t) ((i & 0x00ff) >> 0);
+}
+
+/**
+ * Writes a 32-bit integer into memory in little-endian byte order.
+ *
+ * \param[in] p Pointer where to write the integer to.
+ * \param[in] i The 32-bit integer to write.
+ */
+void write32(uint8_t* p, uint32_t i)
+{
+ p[3] = (uint8_t) ((i & 0xff000000) >> 24);
+ p[2] = (uint8_t) ((i & 0x00ff0000) >> 16);
+ p[1] = (uint8_t) ((i & 0x0000ff00) >> 8);
+ p[0] = (uint8_t) ((i & 0x000000ff) >> 0);
+}
+
+/**
+ * @}
+ */
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef BYTEORDERING_H
+#define BYTEORDERING_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup byteordering
+ *
+ * @{
+ */
+/**
+ * \file
+ * Byte-order handling header (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+#define SWAP16(val) ((((uint16_t) (val)) << 8) | \
+ (((uint16_t) (val)) >> 8) \
+ )
+#define SWAP32(val) (((((uint32_t) (val)) & 0x000000ff) << 24) | \
+ ((((uint32_t) (val)) & 0x0000ff00) << 8) | \
+ ((((uint32_t) (val)) & 0x00ff0000) >> 8) | \
+ ((((uint32_t) (val)) & 0xff000000) >> 24) \
+ )
+
+#if LITTLE_ENDIAN || __AVR__
+#define SWAP_NEEDED 0
+#elif BIG_ENDIAN
+#define SWAP_NEEDED 1
+#else
+#error "Endianess undefined! Please define LITTLE_ENDIAN=1 or BIG_ENDIAN=1."
+#endif
+
+/**
+ * \def HTOL16(val)
+ *
+ * Converts a 16-bit integer from host byte order to little-endian byte order.
+ *
+ * Use this macro for compile time constants only. For variable values
+ * use the function htol16() instead. This saves code size.
+ *
+ * \param[in] val A 16-bit integer in host byte order.
+ * \returns The given 16-bit integer converted to little-endian byte order.
+ */
+/**
+ * \def HTOL32(val)
+ *
+ * Converts a 32-bit integer from host byte order to little-endian byte order.
+ *
+ * Use this macro for compile time constants only. For variable values
+ * use the function htol32() instead. This saves code size.
+ *
+ * \param[in] val A 32-bit integer in host byte order.
+ * \returns The given 32-bit integer converted to little-endian byte order.
+ */
+/**
+ * \def LTOH16(val)
+ *
+ * Converts a 16-bit integer from little-endian byte order to host byte order.
+ *
+ * Use this macro for compile time constants only. For variable values
+ * use the function ltoh16() instead. This saves code size.
+ *
+ * \param[in] val A 16-bit integer in little-endian byte order.
+ * \returns The given 16-bit integer converted to host byte order.
+ */
+/**
+ * \def LTOH32(val)
+ *
+ * Converts a 32-bit integer from little-endian byte order to host byte order.
+ *
+ * Use this macro for compile time constants only. For variable values
+ * use the function ltoh32() instead. This saves code size.
+ *
+ * \param[in] val A 32-bit integer in little-endian byte order.
+ * \returns The given 32-bit integer converted to host byte order.
+ */
+
+#if SWAP_NEEDED
+#define HTOL16(val) SWAP16(val)
+#define HTOL32(val) SWAP32(val)
+#define LTOH16(val) SWAP16(val)
+#define LTOH32(val) SWAP32(val)
+#else
+#define HTOL16(val) (val)
+#define HTOL32(val) (val)
+#define LTOH16(val) (val)
+#define LTOH32(val) (val)
+#endif
+
+#if DOXYGEN
+
+/**
+ * Converts a 16-bit integer from host byte order to little-endian byte order.
+ *
+ * Use this function on variable values instead of the
+ * macro HTOL16(). This saves code size.
+ *
+ * \param[in] h A 16-bit integer in host byte order.
+ * \returns The given 16-bit integer converted to little-endian byte order.
+ */
+uint16_t htol16(uint16_t h);
+
+/**
+ * Converts a 32-bit integer from host byte order to little-endian byte order.
+ *
+ * Use this function on variable values instead of the
+ * macro HTOL32(). This saves code size.
+ *
+ * \param[in] h A 32-bit integer in host byte order.
+ * \returns The given 32-bit integer converted to little-endian byte order.
+ */
+uint32_t htol32(uint32_t h);
+
+/**
+ * Converts a 16-bit integer from little-endian byte order to host byte order.
+ *
+ * Use this function on variable values instead of the
+ * macro LTOH16(). This saves code size.
+ *
+ * \param[in] l A 16-bit integer in little-endian byte order.
+ * \returns The given 16-bit integer converted to host byte order.
+ */
+uint16_t ltoh16(uint16_t l);
+
+/**
+ * Converts a 32-bit integer from little-endian byte order to host byte order.
+ *
+ * Use this function on variable values instead of the
+ * macro LTOH32(). This saves code size.
+ *
+ * \param[in] l A 32-bit integer in little-endian byte order.
+ * \returns The given 32-bit integer converted to host byte order.
+ */
+uint32_t ltoh32(uint32_t l);
+
+#elif SWAP_NEEDED
+
+#define htol16(h) swap16(h)
+#define htol32(h) swap32(h)
+#define ltoh16(l) swap16(l)
+#define ltoh32(l) swap32(l)
+
+#else
+
+#define htol16(h) (h)
+#define htol32(h) (h)
+#define ltoh16(l) (l)
+#define ltoh32(l) (l)
+
+#endif
+
+uint16_t read16(const uint8_t* p);
+uint32_t read32(const uint8_t* p);
+void write16(uint8_t* p, uint16_t i);
+void write32(uint8_t* p, uint32_t i);
+
+/**
+ * @}
+ */
+
+#if SWAP_NEEDED
+uint16_t swap16(uint16_t i);
+uint32_t swap32(uint32_t i);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+#include <aversive/queue.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <uart.h>
+
+#include "callout.h"
+#include "main.h"
+#include "cmdline.h"
+
+#define FLUSH_LOGS_MS 1000 /* every second */
+#define LOG_PER_SEC_MAX 10
+
+extern const parse_ctx_t PROGMEM main_ctx[];
+
+static struct callout flush_log_timer;
+static uint8_t log_count;
+
+int cmdline_dev_send(char c, FILE* f)
+{
+ (void)f;
+ uart_send(CMDLINE_UART, c);
+ return 0;
+}
+
+int cmdline_dev_recv(FILE* f)
+{
+ int16_t c;
+
+ (void)f;
+ c = uart_recv_nowait(CMDLINE_UART);
+ if (c < 0)
+ return _FDEV_EOF;
+
+ return c;
+}
+
+
+int xbee_dev_send(char c, FILE* f)
+{
+ (void)f;
+ uart_send(XBEE_UART, c);
+ return 0;
+}
+
+int xbee_dev_recv(FILE* f)
+{
+ int16_t c;
+
+ (void)f;
+ c = uart_recv_nowait(XBEE_UART);
+ if (c < 0)
+ return _FDEV_EOF;
+
+ return c;
+}
+
+void cmdline_valid_buffer(const char *buf, uint8_t size)
+{
+ int8_t ret;
+ PGM_P ctx = (PGM_P)main_ctx;
+
+ (void)size;
+ ret = parse(ctx, buf);
+ if (ret == PARSE_AMBIGUOUS)
+ printf_P(PSTR("Ambiguous command\r\n"));
+ else if (ret == PARSE_NOMATCH)
+ printf_P(PSTR("Command not found\r\n"));
+ else if (ret == PARSE_BAD_ARGS)
+ printf_P(PSTR("Bad arguments\r\n"));
+}
+
+static int8_t
+complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
+ int16_t *state)
+{
+ PGM_P ctx = (PGM_P)main_ctx;
+ return complete(ctx, buf, state, dstbuf, dstsize);
+}
+
+
+void cmdline_write_char(char c)
+{
+ cmdline_dev_send(c, NULL);
+}
+
+
+/* sending "pop" on cmdline uart resets the robot */
+void emergency(char c)
+{
+ static uint8_t i = 0;
+
+ if ((i == 0 && c == 'p') ||
+ (i == 1 && c == 'o') ||
+ (i == 2 && c == 'p'))
+ i++;
+ else if ( !(i == 1 && c == 'p') )
+ i = 0;
+ if (i == 3) {
+ //bootloader();
+ reset();
+ }
+}
+
+/* log function, configured dynamically */
+void mylog(struct error * e, ...)
+{
+#ifndef HOST_VERSION
+ u16 stream_flags = stdout->flags;
+#endif
+ va_list ap;
+ uint8_t i, flags;
+ uint32_t ms;
+ uint8_t prio;
+
+ /* too many logs */
+ if (log_count >= LOG_PER_SEC_MAX)
+ return;
+
+ /* higher log value means lower criticity */
+ if (e->severity > ERROR_SEVERITY_ERROR) {
+ if (imuboard.log_level < e->severity)
+ return;
+
+ for (i=0; i<NB_LOGS+1; i++)
+ if (imuboard.logs[i] == e->err_num)
+ break;
+ if (i == NB_LOGS+1)
+ return;
+ }
+
+ /* get time */
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ IRQ_UNLOCK(flags);
+
+ /* prevent flush log to occur */
+ prio = callout_mgr_set_prio(&imuboard.intr_cm,
+ LOW_PRIO);
+
+ /* display the log */
+ va_start(ap, e);
+ printf_P(PSTR("%d.%.3d: "), (int)(ms/1000UL), (int)(ms%1000UL));
+ vfprintf_P(stdout, e->text, ap);
+ printf_P(PSTR("\r\n"));
+ va_end(ap);
+
+#ifndef HOST_VERSION
+ stdout->flags = stream_flags;
+#endif
+ callout_mgr_restore_prio(&imuboard.intr_cm, prio);
+}
+
+static void flush_logs_cb(struct callout_mgr *cm, struct callout *tim,
+ void *arg)
+{
+ (void)cm;
+ (void)tim;
+ (void)arg;
+
+ if (log_count == LOG_PER_SEC_MAX)
+ printf_P("some logs were dropped\n");
+ callout_reschedule(&imuboard.intr_cm, &flush_log_timer,
+ FLUSH_LOGS_MS);
+}
+
+
+int cmdline_poll(void)
+{
+ const char *history, *buffer;
+ int8_t ret, same = 0;
+ int16_t c;
+
+ c = cmdline_dev_recv(NULL);
+ if (c < 0)
+ return -1;
+
+ ret = rdline_char_in(&imuboard.rdl, c);
+ if (ret == 1) {
+ buffer = rdline_get_buffer(&imuboard.rdl);
+ history = rdline_get_history_item(&imuboard.rdl, 0);
+ if (history) {
+ same = !memcmp(buffer, history, strlen(history)) &&
+ buffer[strlen(history)] == '\n';
+ }
+ else
+ same = 0;
+ if (strlen(buffer) > 1 && !same)
+ rdline_add_history(&imuboard.rdl, buffer);
+
+ if (imuboard.rdl.status != RDLINE_STOPPED)
+ rdline_newline(&imuboard.rdl, imuboard.prompt);
+ }
+
+ return 0;
+}
+
+void cmdline_init(void)
+{
+ /* init command line */
+ rdline_init(&imuboard.rdl, cmdline_write_char, cmdline_valid_buffer,
+ complete_buffer);
+ snprintf_P(imuboard.prompt, sizeof(imuboard.prompt),
+ PSTR("imu > "));
+
+ /* load a timer for flushing logs */
+ callout_init(&flush_log_timer, flush_logs_cb, NULL, LOW_PRIO);
+ callout_schedule(&imuboard.intr_cm, &flush_log_timer, FLUSH_LOGS_MS);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.h,v 1.4 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#ifndef _CMDLINE_H_
+#define _CMDLINE_H_
+
+#define CMDLINE_UART 1
+#define XBEE_UART 0
+
+void cmdline_init(void);
+
+/* uart rx callback for reset() */
+void emergency(char c);
+
+/* log function */
+void mylog(struct error * e, ...);
+
+/* poll cmdline */
+int cmdline_poll(void);
+
+int cmdline_dev_send(char c, FILE* f);
+int cmdline_dev_recv(FILE* f);
+void cmdline_write_char(char c);
+void cmdline_valid_buffer(const char *buf, uint8_t size);
+
+int xbee_dev_send(char c, FILE* f);
+int xbee_dev_recv(FILE* f);
+
+static inline uint8_t cmdline_keypressed(void)
+{
+ return (uart_recv_nowait(CMDLINE_UART) != -1);
+}
+
+static inline int16_t cmdline_getchar(void)
+{
+ return uart_recv_nowait(CMDLINE_UART);
+}
+
+static inline uint8_t cmdline_getchar_wait(void)
+{
+ return uart_recv(CMDLINE_UART);
+}
+
+#endif /* _CMDLINE_H_ */
--- /dev/null
+/*
+ * Copyright Droids Corporation (2011)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <inttypes.h>
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/endian.h>
+#include <aversive/error.h>
+#include <aversive/wait.h>
+#include <parse.h>
+#include <rdline.h>
+#include <parse_string.h>
+#include <parse_num.h>
+#include <uart.h>
+#include <xbee.h>
+#include <callout.h>
+
+#include "main.h"
+#include "cmdline.h"
+#include "eeprom_config.h"
+
+/* commands_gen.c */
+extern const parse_inst_t PROGMEM cmd_reset;
+extern const parse_inst_t PROGMEM cmd_bootloader;
+extern const parse_inst_t PROGMEM cmd_log;
+extern const parse_inst_t PROGMEM cmd_log_show;
+extern const parse_inst_t PROGMEM cmd_log_type;
+extern const parse_inst_t PROGMEM cmd_stack_space;
+extern const parse_inst_t PROGMEM cmd_callout;
+
+/**********************************************************/
+
+/* this structure is filled when cmd_test_eeprom_config is parsed successfully */
+struct cmd_test_eeprom_config_result {
+ fixed_string_t arg0;
+};
+
+static void cmd_test_eeprom_config_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+
+ eeprom_dump_cmds();
+ eeprom_append_cmd("salut1\n");
+ eeprom_dump_cmds();
+ eeprom_append_cmd("salut2\n");
+ eeprom_append_cmd("salut3\n");
+ eeprom_append_cmd("salut4\n");
+ eeprom_dump_cmds();
+ eeprom_insert_cmd_before("coin\n", 0);
+ eeprom_insert_cmd_before("coin2\n", 2);
+ eeprom_dump_cmds();
+ eeprom_delete_cmd(2);
+ eeprom_delete_cmd(0);
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_test_eeprom_config_arg0[] = "test_eeprom_config";
+const parse_token_string_t PROGMEM cmd_test_eeprom_config_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_test_eeprom_config_result, arg0,
+ str_test_eeprom_config_arg0);
+
+const char PROGMEM help_test_eeprom_config[] = "Test the eeprom configuration";
+const parse_inst_t PROGMEM cmd_test_eeprom_config = {
+ .f = cmd_test_eeprom_config_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_test_eeprom_config,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_test_eeprom_config_arg0,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_eeprom_del_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+ uint8_t n;
+};
+
+static void cmd_eeprom_del_parsed(void *parsed_result,
+ void *data)
+{
+ struct cmd_eeprom_del_result *res = parsed_result;
+
+ (void)data;
+ if (eeprom_delete_cmd(res->n) < 0)
+ printf_P(PSTR("cannot delete command\n"));
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_eeprom_del_eeprom[] = "eeprom";
+const parse_token_string_t PROGMEM cmd_eeprom_del_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_del_result, cmd,
+ str_eeprom_del_eeprom);
+const char PROGMEM str_eeprom_del_del[] = "del";
+const parse_token_string_t PROGMEM cmd_eeprom_del_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_del_result, action,
+ str_eeprom_del_del);
+const parse_token_num_t PROGMEM cmd_eeprom_del_num =
+ TOKEN_NUM_INITIALIZER(struct cmd_eeprom_del_result, n,
+ UINT8);
+
+const char PROGMEM help_eeprom_del[] = "delete an eeprom init command";
+const parse_inst_t PROGMEM cmd_eeprom_del = {
+ .f = cmd_eeprom_del_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_eeprom_del,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_del_cmd,
+ (PGM_P)&cmd_eeprom_del_action,
+ (PGM_P)&cmd_eeprom_del_num,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_eeprom_add_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+ uint8_t n;
+};
+
+static void cmd_eeprom_add_parsed(void *parsed_result,
+ void *data)
+{
+ struct cmd_eeprom_add_result *res = parsed_result;
+ struct rdline rdl;
+ const char *buffer;
+ int8_t ret;
+ int16_t c;
+
+ rdline_init(&rdl, cmdline_write_char, NULL, NULL);
+ rdline_newline(&rdl, "> ");
+
+ while (1) {
+ c = cmdline_dev_recv(NULL);
+ if (c < 0)
+ continue;
+
+ ret = rdline_char_in(&rdl, c);
+ if (ret == -2) {
+ printf_P(PSTR("abort\n"));
+ return;
+ }
+ if (ret == 1)
+ break;
+ }
+
+ buffer = rdline_get_buffer(&rdl);
+ if (data == NULL)
+ eeprom_insert_cmd_before(buffer, res->n);
+ else
+ eeprom_append_cmd(buffer);
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_eeprom_add_eeprom[] = "eeprom";
+const parse_token_string_t PROGMEM cmd_eeprom_add_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_add_result, cmd,
+ str_eeprom_add_eeprom);
+const char PROGMEM str_eeprom_add_add[] = "add";
+const parse_token_string_t PROGMEM cmd_eeprom_add_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_add_result, action,
+ str_eeprom_add_add);
+const parse_token_num_t PROGMEM cmd_eeprom_add_num =
+ TOKEN_NUM_INITIALIZER(struct cmd_eeprom_add_result, n,
+ UINT8);
+
+const char PROGMEM help_eeprom_add[] = "insert an eeprom init command";
+const parse_inst_t PROGMEM cmd_eeprom_add = {
+ .f = cmd_eeprom_add_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_eeprom_add,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_add_cmd,
+ (PGM_P)&cmd_eeprom_add_action,
+ (PGM_P)&cmd_eeprom_add_num,
+ NULL,
+ },
+};
+
+const char PROGMEM help_eeprom_add2[] = "append an eeprom init command";
+const parse_inst_t PROGMEM cmd_eeprom_add2 = {
+ .f = cmd_eeprom_add_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_eeprom_add2,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_add_cmd,
+ (PGM_P)&cmd_eeprom_add_action,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_eeprom_list_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+};
+
+static void cmd_eeprom_list_parsed(void *parsed_result,
+ void *data)
+{
+ (void)parsed_result;
+ (void)data;
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_eeprom_list_eeprom[] = "eeprom";
+const parse_token_string_t PROGMEM cmd_eeprom_list_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_list_result, cmd,
+ str_eeprom_list_eeprom);
+const char PROGMEM str_eeprom_list_list[] = "list";
+const parse_token_string_t PROGMEM cmd_eeprom_list_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_list_result, action,
+ str_eeprom_list_list);
+
+const char PROGMEM help_eeprom_list[] = "list all eeprom init commands";
+const parse_inst_t PROGMEM cmd_eeprom_list = {
+ .f = cmd_eeprom_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_eeprom_list,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_list_cmd,
+ (PGM_P)&cmd_eeprom_list_action,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+/* in progmem */
+const parse_ctx_t PROGMEM main_ctx[] = {
+
+ /* commands_gen.c */
+ &cmd_reset,
+ &cmd_bootloader,
+ &cmd_log,
+ &cmd_log_show,
+ &cmd_log_type,
+ &cmd_stack_space,
+ &cmd_callout,
+ &cmd_test_eeprom_config,
+ &cmd_eeprom_del,
+ &cmd_eeprom_add,
+ &cmd_eeprom_add2,
+ &cmd_eeprom_list,
+ NULL,
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2011)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_gen.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <aversive/queue.h>
+
+#include <uart.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include <diagnostic.h>
+
+#include "callout.h"
+#include "main.h"
+#include "cmdline.h"
+
+/**********************************************************/
+/* Reset */
+
+/* this structure is filled when cmd_reset is parsed successfully */
+struct cmd_reset_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_reset is parsed successfully */
+static void cmd_reset_parsed(void * parsed_result, void * data)
+{
+ (void)parsed_result;
+ (void)data;
+#ifdef HOST_VERSION
+ hostsim_exit();
+#endif
+ reset();
+}
+
+const char PROGMEM str_reset_arg0[] = "reset";
+const parse_token_string_t PROGMEM cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
+
+const char PROGMEM help_reset[] = "Reset the board";
+const parse_inst_t PROGMEM cmd_reset = {
+ .f = cmd_reset_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_reset,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_reset_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Bootloader */
+
+/* this structure is filled when cmd_bootloader is parsed successfully */
+struct cmd_bootloader_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_bootloader is parsed successfully */
+static void cmd_bootloader_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+#ifndef HOST_VERSION
+ bootloader();
+#else
+ printf("not implemented\n");
+#endif
+}
+
+const char PROGMEM str_bootloader_arg0[] = "bootloader";
+const parse_token_string_t PROGMEM cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
+
+const char PROGMEM help_bootloader[] = "Launch the bootloader";
+const parse_inst_t PROGMEM cmd_bootloader = {
+ .f = cmd_bootloader_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_bootloader,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_bootloader_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Callout show */
+
+/* this structure is filled when cmd_callout is parsed successfully */
+struct cmd_callout_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_callout is parsed successfully */
+static void cmd_callout_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+ callout_dump_stats(&imuboard.intr_cm);
+}
+
+const char PROGMEM str_callout_arg0[] = "callout";
+const parse_token_string_t PROGMEM cmd_callout_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_callout_result, arg0, str_callout_arg0);
+const char PROGMEM str_callout_arg1[] = "show";
+const parse_token_string_t PROGMEM cmd_callout_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_callout_result, arg1, str_callout_arg1);
+
+const char PROGMEM help_callout[] = "Show callout events";
+const parse_inst_t PROGMEM cmd_callout = {
+ .f = cmd_callout_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_callout,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_callout_arg0,
+ (PGM_P)&cmd_callout_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+ fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const char PROGMEM uart_log[] = "uart";
+static const char PROGMEM i2c_log[] = "i2c";
+static const char PROGMEM default_log[] = "default";
+static const char PROGMEM xbee_log[] = "xbee";
+static const char PROGMEM rc_proto_log[] = "rc_proto";
+
+struct log_name_and_num {
+ const char *name;
+ uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+ { uart_log, E_UART },
+ { i2c_log, E_I2C },
+ { default_log, E_USER_DEFAULT },
+ { xbee_log, E_USER_XBEE },
+ { rc_proto_log, E_USER_RC_PROTO },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (!strcmp_P(s, log_name_and_num[i].name)) {
+ return log_name_and_num[i].num;
+ }
+ }
+ return 0;
+}
+
+const char *
+log_num2name(uint8_t num)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (num == log_name_and_num[i].num) {
+ return log_name_and_num[i].name;
+ }
+ }
+ return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+ uint8_t i, empty=1;
+ const char *name;
+
+ printf_P(PSTR("log level is %d\r\n"), imuboard.log_level);
+ for (i=0; i<NB_LOGS; i++) {
+ name = log_num2name(imuboard.logs[i]);
+ if (name) {
+#ifdef HOST_VERSION
+ printf_P(PSTR("log type %s is on\r\n"), name);
+#else
+ printf_P(PSTR("log type %S is on\r\n"), name);
+#endif
+ empty = 0;
+ }
+ }
+ if (empty)
+ printf_P(PSTR("no log configured\r\n"));
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, void *data)
+{
+ struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+ (void)data;
+ if (!strcmp_P(res->arg1, PSTR("level"))) {
+ imuboard.log_level = res->arg2;
+ }
+
+ /* else it is a show */
+ cmd_log_do_show();
+}
+
+const char PROGMEM str_log_arg0[] = "log";
+const parse_token_string_t PROGMEM cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
+const char PROGMEM str_log_arg1[] = "level";
+const parse_token_string_t PROGMEM cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
+const parse_token_num_t PROGMEM cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT8);
+
+const char PROGMEM help_log[] = "Set log options: level (0 -> 5)";
+const parse_inst_t PROGMEM cmd_log = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_log_arg0,
+ (PGM_P)&cmd_log_arg1,
+ (PGM_P)&cmd_log_arg2,
+ NULL,
+ },
+};
+
+const char PROGMEM str_log_arg1_show[] = "show";
+const parse_token_string_t PROGMEM cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
+
+const char PROGMEM help_log_show[] = "Show configured logs";
+const parse_inst_t PROGMEM cmd_log_show = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_show,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_log_arg0,
+ (PGM_P)&cmd_log_arg1_show,
+ NULL,
+ },
+};
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_type_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_type_parsed(void * parsed_result, void *data)
+{
+ struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
+ uint8_t lognum;
+ uint8_t i;
+
+ (void)data;
+
+ lognum = log_name2num(res->arg2);
+ if (lognum == 0) {
+ printf_P(PSTR("Cannot find log num\r\n"));
+ return;
+ }
+
+ if (!strcmp_P(res->arg3, PSTR("on"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (imuboard.logs[i] == lognum) {
+ printf_P(PSTR("Already on\r\n"));
+ return;
+ }
+ }
+ for (i=0; i<NB_LOGS; i++) {
+ if (imuboard.logs[i] == 0) {
+ imuboard.logs[i] = lognum;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("no more room\r\n"));
+ }
+ }
+ else if (!strcmp_P(res->arg3, PSTR("off"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (imuboard.logs[i] == lognum) {
+ imuboard.logs[i] = 0;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("already off\r\n"));
+ }
+ }
+ cmd_log_do_show();
+}
+
+const char PROGMEM str_log_arg1_type[] = "type";
+const parse_token_string_t PROGMEM cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+const char PROGMEM str_log_arg2_type[] =
+ "uart#i2c#i2cproto#default#xbee#rc_proto";
+const parse_token_string_t PROGMEM cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+const char PROGMEM str_log_arg3[] = "on#off";
+const parse_token_string_t PROGMEM cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+const char PROGMEM help_log_type[] = "Set log type";
+const parse_inst_t PROGMEM cmd_log_type = {
+ .f = cmd_log_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_type,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_log_arg0,
+ (PGM_P)&cmd_log_arg1_type,
+ (PGM_P)&cmd_log_arg2_type,
+ (PGM_P)&cmd_log_arg3,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ printf("res stack: %d\r\n", min_stack_space_available());
+#endif
+}
+
+const char PROGMEM str_stack_space_arg0[] = "stack_space";
+const parse_token_string_t PROGMEM cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+const char PROGMEM help_stack_space[] = "Display remaining stack space";
+const parse_inst_t PROGMEM cmd_stack_space = {
+ .f = cmd_stack_space_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_stack_space,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_stack_space_arg0,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: diagnostic_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _DEBUG_CONFIG_
+#define _DEBUG_CONFIG_ 1.0 // version
+
+
+/** port line definition for the show_int_loop() function */
+/* undefine it to disable this functionnality */
+#define INTERRUPT_SHOW_PORT PORTA
+#define INTERRUPT_SHOW_BIT 3
+
+
+
+/** memory mark for the min_stack_space_available() function
+ the ram is filled with this value after a reset ! */
+#define MARK 0x55
+
+/** the mark is inserted in whole RAM if this is enabled
+ (could lead to problems if you need to hold values through a reset...)
+ so it's better to disable it.
+ stack counting is not affected */
+//#define DIAG_FILL_ENTIRE_RAM
+
+
+#endif //_DEBUG_CONFIG_
--- /dev/null
+/*
+ * Copyright 2013 Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include <stdint.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/eeprom.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+
+
+#include "cmdline.h"
+#include "eeprom_config.h"
+
+/* load configuration from eeprom */
+int8_t eeprom_load_config(void)
+{
+ struct eeprom_config *e = NULL;
+ struct eeprom_cmd cmd;
+ uint32_t magic;
+ uint8_t i, max;
+
+ eeprom_read_block(&magic, &e->magic, sizeof(magic));
+ if (magic != EEPROM_CONFIG_MAGIC) {
+ printf_P(PSTR("no EEPROM config\n"));
+ eeprom_set_ncmds(0);
+ return 0;
+ }
+
+ max = eeprom_get_ncmds();
+ for (i = 0; i < max; i++) {
+ eeprom_get_cmd(&cmd, i);
+ printf_P(PSTR("%s"), cmd.buf);
+ cmdline_valid_buffer(cmd.buf, strlen(cmd.buf));
+ }
+
+ return -1;
+}
+
+uint8_t eeprom_get_ncmds(void)
+{
+ struct eeprom_config *e = NULL;
+ uint8_t ncmds;
+
+ eeprom_read_block(&ncmds, &e->ncmds, sizeof(ncmds));
+ return ncmds;
+}
+
+void eeprom_set_ncmds(uint8_t ncmds)
+{
+ struct eeprom_config *e = NULL;
+ uint32_t magic = EEPROM_CONFIG_MAGIC;
+ eeprom_update_block(&ncmds, &e->ncmds, sizeof(ncmds));
+ eeprom_update_block(&magic, &e->magic, sizeof(magic));
+}
+
+/* fill cmd struct with the n-th command from eeprom, no check is done
+ * on index or size. The \0 is added at the end of the string. */
+void eeprom_get_cmd(struct eeprom_cmd *cmd, uint8_t n)
+{
+ struct eeprom_config *e = NULL;
+
+ eeprom_read_block(cmd, &e->cmds[n], sizeof(*cmd));
+ cmd->buf[EEPROM_CMD_SIZE-1] = '\0';
+}
+
+/* fill n-th command of eeprom from struct, no check is done on index
+ * or size */
+void eeprom_set_cmd(struct eeprom_cmd *cmd, uint8_t n)
+{
+ struct eeprom_config *e = NULL;
+
+ eeprom_update_block(cmd, &e->cmds[n], sizeof(*cmd));
+}
+
+void eeprom_dump_cmds(void)
+{
+ uint8_t i, max;
+ struct eeprom_cmd cmd;
+
+ printf_P(PSTR("init commands:\n"));
+ max = eeprom_get_ncmds();
+ for (i = 0; i < max; i++) {
+ eeprom_get_cmd(&cmd, i);
+ printf_P(PSTR("%.2d: %s"), i, cmd.buf);
+ }
+}
+
+int8_t eeprom_insert_cmd_before(const char *str, uint8_t n)
+{
+ uint8_t i, max;
+ struct eeprom_cmd cmd;
+
+ if (strlen(str) >= EEPROM_CMD_SIZE)
+ return -1;
+
+ max = eeprom_get_ncmds();
+ if (n > max)
+ return -1;
+ if (max >= EEPROM_N_CMD_MAX)
+ return -1;
+
+ for (i = max; i > n; i--) {
+ eeprom_get_cmd(&cmd, i-1);
+ eeprom_set_cmd(&cmd, i);
+ }
+
+ snprintf(cmd.buf, sizeof(cmd.buf), "%s", str);
+ eeprom_set_cmd(&cmd, n);
+ eeprom_set_ncmds(max + 1);
+ return 0;
+}
+
+int8_t eeprom_append_cmd(const char *str)
+{
+ uint8_t max;
+
+ max = eeprom_get_ncmds();
+ return eeprom_insert_cmd_before(str, max);
+}
+
+int8_t eeprom_delete_cmd(uint8_t n)
+{
+ uint8_t i, max;
+ struct eeprom_cmd cmd;
+
+ max = eeprom_get_ncmds();
+ if (n >= max)
+ return -1;
+
+ for (i = n; i < max-1; i++) {
+ eeprom_get_cmd(&cmd, i+1);
+ eeprom_set_cmd(&cmd, i);
+ }
+
+ eeprom_set_ncmds(max - 1);
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright 2013 Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+#ifndef _EEPROM_CONFIG_H_
+#define _EEPROM_CONFIG_H_
+
+#define EEPROM_CONFIG_MAGIC 0x666bea57
+#define EEPROM_CMD_SIZE 64
+#define EEPROM_N_CMD_MAX 16
+
+struct eeprom_cmd {
+ char buf[EEPROM_CMD_SIZE];
+};
+
+struct eeprom_config
+{
+ uint32_t magic;
+ uint8_t ncmds;
+ struct eeprom_cmd cmds[EEPROM_N_CMD_MAX];
+};
+
+int8_t eeprom_load_config(void);
+uint8_t eeprom_get_ncmds(void);
+void eeprom_set_ncmds(uint8_t ncmds);
+void eeprom_get_cmd(struct eeprom_cmd *cmd, uint8_t n);
+void eeprom_set_cmd(struct eeprom_cmd *cmd, uint8_t n);
+void eeprom_dump_cmds(void);
+int8_t eeprom_insert_cmd_before(const char *str, uint8_t n);
+int8_t eeprom_append_cmd(const char *str);
+int8_t eeprom_delete_cmd(uint8_t n);
+
+#endif
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef _ERROR_CONFIG_
+#define _ERROR_CONFIG_
+
+/** enable the dump of the comment */
+#define ERROR_DUMP_TEXTLOG
+
+/** enable the dump of filename and line number */
+//#define ERROR_DUMP_FILE_LINE
+
+#endif
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#include "byteordering.h"
+#include "partition.h"
+#include "fat.h"
+#include "fat_config.h"
+#include "sd-reader_config.h"
+
+#include <string.h>
+
+#if USE_DYNAMIC_MEMORY
+ #include <stdlib.h>
+#endif
+
+/**
+ * \addtogroup fat FAT support
+ *
+ * This module implements FAT16/FAT32 read and write access.
+ *
+ * The following features are supported:
+ * - File names up to 31 characters long.
+ * - Unlimited depth of subdirectories.
+ * - Short 8.3 and long filenames.
+ * - Creating and deleting files.
+ * - Reading and writing from and to files.
+ * - File resizing.
+ * - File sizes of up to 4 gigabytes.
+ *
+ * @{
+ */
+/**
+ * \file
+ * FAT implementation (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+/**
+ * \addtogroup fat_config FAT configuration
+ * Preprocessor defines to configure the FAT implementation.
+ */
+
+/**
+ * \addtogroup fat_fs FAT access
+ * Basic functions for handling a FAT filesystem.
+ */
+
+/**
+ * \addtogroup fat_file FAT file functions
+ * Functions for managing files.
+ */
+
+/**
+ * \addtogroup fat_dir FAT directory functions
+ * Functions for managing directories.
+ */
+
+/**
+ * @}
+ */
+
+#define FAT16_CLUSTER_FREE 0x0000
+#define FAT16_CLUSTER_RESERVED_MIN 0xfff0
+#define FAT16_CLUSTER_RESERVED_MAX 0xfff6
+#define FAT16_CLUSTER_BAD 0xfff7
+#define FAT16_CLUSTER_LAST_MIN 0xfff8
+#define FAT16_CLUSTER_LAST_MAX 0xffff
+
+#define FAT32_CLUSTER_FREE 0x00000000
+#define FAT32_CLUSTER_RESERVED_MIN 0x0ffffff0
+#define FAT32_CLUSTER_RESERVED_MAX 0x0ffffff6
+#define FAT32_CLUSTER_BAD 0x0ffffff7
+#define FAT32_CLUSTER_LAST_MIN 0x0ffffff8
+#define FAT32_CLUSTER_LAST_MAX 0x0fffffff
+
+#define FAT_DIRENTRY_DELETED 0xe5
+#define FAT_DIRENTRY_LFNLAST (1 << 6)
+#define FAT_DIRENTRY_LFNSEQMASK ((1 << 6) - 1)
+
+/* Each entry within the directory table has a size of 32 bytes
+ * and either contains a 8.3 DOS-style file name or a part of a
+ * long file name, which may consist of several directory table
+ * entries at once.
+ *
+ * multi-byte integer values are stored little-endian!
+ *
+ * 8.3 file name entry:
+ * ====================
+ * offset length description
+ * 0 8 name (space padded)
+ * 8 3 extension (space padded)
+ * 11 1 attributes (FAT_ATTRIB_*)
+ *
+ * long file name (lfn) entry ordering for a single file name:
+ * ===========================================================
+ * LFN entry n
+ * ...
+ * LFN entry 2
+ * LFN entry 1
+ * 8.3 entry (see above)
+ *
+ * lfn entry:
+ * ==========
+ * offset length description
+ * 0 1 ordinal field
+ * 1 2 unicode character 1
+ * 3 3 unicode character 2
+ * 5 3 unicode character 3
+ * 7 3 unicode character 4
+ * 9 3 unicode character 5
+ * 11 1 attribute (always 0x0f)
+ * 12 1 type (reserved, always 0)
+ * 13 1 checksum
+ * 14 2 unicode character 6
+ * 16 2 unicode character 7
+ * 18 2 unicode character 8
+ * 20 2 unicode character 9
+ * 22 2 unicode character 10
+ * 24 2 unicode character 11
+ * 26 2 cluster (unused, always 0)
+ * 28 2 unicode character 12
+ * 30 2 unicode character 13
+ *
+ * The ordinal field contains a descending number, from n to 1.
+ * For the n'th lfn entry the ordinal field is or'ed with 0x40.
+ * For deleted lfn entries, the ordinal field is set to 0xe5.
+ */
+
+struct fat_header_struct
+{
+ offset_t size;
+
+ offset_t fat_offset;
+ uint32_t fat_size;
+
+ uint16_t sector_size;
+ uint16_t cluster_size;
+
+ offset_t cluster_zero_offset;
+
+ offset_t root_dir_offset;
+#if FAT_FAT32_SUPPORT
+ cluster_t root_dir_cluster;
+#endif
+};
+
+struct fat_fs_struct
+{
+ struct partition_struct* partition;
+ struct fat_header_struct header;
+ cluster_t cluster_free;
+};
+
+struct fat_file_struct
+{
+ struct fat_fs_struct* fs;
+ struct fat_dir_entry_struct dir_entry;
+ offset_t pos;
+ cluster_t pos_cluster;
+};
+
+struct fat_dir_struct
+{
+ struct fat_fs_struct* fs;
+ struct fat_dir_entry_struct dir_entry;
+ cluster_t entry_cluster;
+ uint16_t entry_offset;
+};
+
+struct fat_read_dir_callback_arg
+{
+ struct fat_dir_entry_struct* dir_entry;
+ uintptr_t bytes_read;
+#if FAT_LFN_SUPPORT
+ uint8_t checksum;
+#endif
+ uint8_t finished;
+};
+
+struct fat_usage_count_callback_arg
+{
+ cluster_t cluster_count;
+ uintptr_t buffer_size;
+};
+
+#if !USE_DYNAMIC_MEMORY
+static struct fat_fs_struct fat_fs_handles[FAT_FS_COUNT];
+static struct fat_file_struct fat_file_handles[FAT_FILE_COUNT];
+static struct fat_dir_struct fat_dir_handles[FAT_DIR_COUNT];
+#endif
+
+static uint8_t fat_read_header(struct fat_fs_struct* fs);
+static cluster_t fat_get_next_cluster(const struct fat_fs_struct* fs, cluster_t cluster_num);
+static offset_t fat_cluster_offset(const struct fat_fs_struct* fs, cluster_t cluster_num);
+static uint8_t fat_dir_entry_read_callback(uint8_t* buffer, offset_t offset, void* p);
+#if FAT_LFN_SUPPORT
+static uint8_t fat_calc_83_checksum(const uint8_t* file_name_83);
+#endif
+
+static uint8_t fat_get_fs_free_16_callback(uint8_t* buffer, offset_t offset, void* p);
+#if FAT_FAT32_SUPPORT
+static uint8_t fat_get_fs_free_32_callback(uint8_t* buffer, offset_t offset, void* p);
+#endif
+
+#if FAT_WRITE_SUPPORT
+static cluster_t fat_append_clusters(struct fat_fs_struct* fs, cluster_t cluster_num, cluster_t count);
+static uint8_t fat_free_clusters(struct fat_fs_struct* fs, cluster_t cluster_num);
+static uint8_t fat_terminate_clusters(struct fat_fs_struct* fs, cluster_t cluster_num);
+static uint8_t fat_clear_cluster(const struct fat_fs_struct* fs, cluster_t cluster_num);
+static uintptr_t fat_clear_cluster_callback(uint8_t* buffer, offset_t offset, void* p);
+static offset_t fat_find_offset_for_dir_entry(struct fat_fs_struct* fs, const struct fat_dir_struct* parent, const struct fat_dir_entry_struct* dir_entry);
+static uint8_t fat_write_dir_entry(const struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry);
+#if FAT_DATETIME_SUPPORT
+static void fat_set_file_modification_date(struct fat_dir_entry_struct* dir_entry, uint16_t year, uint8_t month, uint8_t day);
+static void fat_set_file_modification_time(struct fat_dir_entry_struct* dir_entry, uint8_t hour, uint8_t min, uint8_t sec);
+#endif
+#endif
+
+/**
+ * \ingroup fat_fs
+ * Opens a FAT filesystem.
+ *
+ * \param[in] partition Discriptor of partition on which the filesystem resides.
+ * \returns 0 on error, a FAT filesystem descriptor on success.
+ * \see fat_close
+ */
+struct fat_fs_struct* fat_open(struct partition_struct* partition)
+{
+ if(!partition ||
+#if FAT_WRITE_SUPPORT
+ !partition->device_write ||
+ !partition->device_write_interval
+#else
+ 0
+#endif
+ )
+ return 0;
+
+#if USE_DYNAMIC_MEMORY
+ struct fat_fs_struct* fs = malloc(sizeof(*fs));
+ if(!fs)
+ return 0;
+#else
+ struct fat_fs_struct* fs = fat_fs_handles;
+ uint8_t i;
+ for(i = 0; i < FAT_FS_COUNT; ++i)
+ {
+ if(!fs->partition)
+ break;
+
+ ++fs;
+ }
+ if(i >= FAT_FS_COUNT)
+ return 0;
+#endif
+
+ memset(fs, 0, sizeof(*fs));
+
+ fs->partition = partition;
+ if(!fat_read_header(fs))
+ {
+#if USE_DYNAMIC_MEMORY
+ free(fs);
+#else
+ fs->partition = 0;
+#endif
+ return 0;
+ }
+
+ return fs;
+}
+
+/**
+ * \ingroup fat_fs
+ * Closes a FAT filesystem.
+ *
+ * When this function returns, the given filesystem descriptor
+ * will be invalid.
+ *
+ * \param[in] fs The filesystem to close.
+ * \see fat_open
+ */
+void fat_close(struct fat_fs_struct* fs)
+{
+ if(!fs)
+ return;
+
+#if USE_DYNAMIC_MEMORY
+ free(fs);
+#else
+ fs->partition = 0;
+#endif
+}
+
+/**
+ * \ingroup fat_fs
+ * Reads and parses the header of a FAT filesystem.
+ *
+ * \param[in,out] fs The filesystem for which to parse the header.
+ * \returns 0 on failure, 1 on success.
+ */
+uint8_t fat_read_header(struct fat_fs_struct* fs)
+{
+ if(!fs)
+ return 0;
+
+ struct partition_struct* partition = fs->partition;
+ if(!partition)
+ return 0;
+
+ /* read fat parameters */
+#if FAT_FAT32_SUPPORT
+ uint8_t buffer[37];
+#else
+ uint8_t buffer[25];
+#endif
+ offset_t partition_offset = (offset_t) partition->offset * 512;
+ if(!partition->device_read(partition_offset + 0x0b, buffer, sizeof(buffer)))
+ return 0;
+
+ uint16_t bytes_per_sector = read16(&buffer[0x00]);
+ uint16_t reserved_sectors = read16(&buffer[0x03]);
+ uint8_t sectors_per_cluster = buffer[0x02];
+ uint8_t fat_copies = buffer[0x05];
+ uint16_t max_root_entries = read16(&buffer[0x06]);
+ uint16_t sector_count_16 = read16(&buffer[0x08]);
+ uint16_t sectors_per_fat = read16(&buffer[0x0b]);
+ uint32_t sector_count = read32(&buffer[0x15]);
+#if FAT_FAT32_SUPPORT
+ uint32_t sectors_per_fat32 = read32(&buffer[0x19]);
+ uint32_t cluster_root_dir = read32(&buffer[0x21]);
+#endif
+
+ if(sector_count == 0)
+ {
+ if(sector_count_16 == 0)
+ /* illegal volume size */
+ return 0;
+ else
+ sector_count = sector_count_16;
+ }
+#if FAT_FAT32_SUPPORT
+ if(sectors_per_fat != 0)
+ sectors_per_fat32 = sectors_per_fat;
+ else if(sectors_per_fat32 == 0)
+ /* this is neither FAT16 nor FAT32 */
+ return 0;
+#else
+ if(sectors_per_fat == 0)
+ /* this is not a FAT16 */
+ return 0;
+#endif
+
+ /* determine the type of FAT we have here */
+ uint32_t data_sector_count = sector_count
+ - reserved_sectors
+#if FAT_FAT32_SUPPORT
+ - sectors_per_fat32 * fat_copies
+#else
+ - (uint32_t) sectors_per_fat * fat_copies
+#endif
+ - ((max_root_entries * 32 + bytes_per_sector - 1) / bytes_per_sector);
+ uint32_t data_cluster_count = data_sector_count / sectors_per_cluster;
+ if(data_cluster_count < 4085)
+ /* this is a FAT12, not supported */
+ return 0;
+ else if(data_cluster_count < 65525)
+ /* this is a FAT16 */
+ partition->type = PARTITION_TYPE_FAT16;
+ else
+ /* this is a FAT32 */
+ partition->type = PARTITION_TYPE_FAT32;
+
+ /* fill header information */
+ struct fat_header_struct* header = &fs->header;
+ memset(header, 0, sizeof(*header));
+
+ header->size = (offset_t) sector_count * bytes_per_sector;
+
+ header->fat_offset = /* jump to partition */
+ partition_offset +
+ /* jump to fat */
+ (offset_t) reserved_sectors * bytes_per_sector;
+ header->fat_size = (data_cluster_count + 2) * (partition->type == PARTITION_TYPE_FAT16 ? 2 : 4);
+
+ header->sector_size = bytes_per_sector;
+ header->cluster_size = (uint16_t) bytes_per_sector * sectors_per_cluster;
+
+#if FAT_FAT32_SUPPORT
+ if(partition->type == PARTITION_TYPE_FAT16)
+#endif
+ {
+ header->root_dir_offset = /* jump to fats */
+ header->fat_offset +
+ /* jump to root directory entries */
+ (offset_t) fat_copies * sectors_per_fat * bytes_per_sector;
+
+ header->cluster_zero_offset = /* jump to root directory entries */
+ header->root_dir_offset +
+ /* skip root directory entries */
+ (offset_t) max_root_entries * 32;
+ }
+#if FAT_FAT32_SUPPORT
+ else
+ {
+ header->cluster_zero_offset = /* jump to fats */
+ header->fat_offset +
+ /* skip fats */
+ (offset_t) fat_copies * sectors_per_fat32 * bytes_per_sector;
+
+ header->root_dir_cluster = cluster_root_dir;
+ }
+#endif
+
+ return 1;
+}
+
+/**
+ * \ingroup fat_fs
+ * Retrieves the next following cluster of a given cluster.
+ *
+ * Using the filesystem file allocation table, this function returns
+ * the number of the cluster containing the data directly following
+ * the data within the cluster with the given number.
+ *
+ * \param[in] fs The filesystem for which to determine the next cluster.
+ * \param[in] cluster_num The number of the cluster for which to determine its successor.
+ * \returns The wanted cluster number, or 0 on error.
+ */
+cluster_t fat_get_next_cluster(const struct fat_fs_struct* fs, cluster_t cluster_num)
+{
+ if(!fs || cluster_num < 2)
+ return 0;
+
+#if FAT_FAT32_SUPPORT
+ if(fs->partition->type == PARTITION_TYPE_FAT32)
+ {
+ /* read appropriate fat entry */
+ uint32_t fat_entry;
+ if(!fs->partition->device_read(fs->header.fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry)))
+ return 0;
+
+ /* determine next cluster from fat */
+ cluster_num = ltoh32(fat_entry);
+
+ if(cluster_num == FAT32_CLUSTER_FREE ||
+ cluster_num == FAT32_CLUSTER_BAD ||
+ (cluster_num >= FAT32_CLUSTER_RESERVED_MIN && cluster_num <= FAT32_CLUSTER_RESERVED_MAX) ||
+ (cluster_num >= FAT32_CLUSTER_LAST_MIN && cluster_num <= FAT32_CLUSTER_LAST_MAX))
+ return 0;
+ }
+ else
+#endif
+ {
+ /* read appropriate fat entry */
+ uint16_t fat_entry;
+ if(!fs->partition->device_read(fs->header.fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry)))
+ return 0;
+
+ /* determine next cluster from fat */
+ cluster_num = ltoh16(fat_entry);
+
+ if(cluster_num == FAT16_CLUSTER_FREE ||
+ cluster_num == FAT16_CLUSTER_BAD ||
+ (cluster_num >= FAT16_CLUSTER_RESERVED_MIN && cluster_num <= FAT16_CLUSTER_RESERVED_MAX) ||
+ (cluster_num >= FAT16_CLUSTER_LAST_MIN && cluster_num <= FAT16_CLUSTER_LAST_MAX))
+ return 0;
+ }
+
+ return cluster_num;
+}
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Appends a new cluster chain to an existing one.
+ *
+ * Set cluster_num to zero to create a completely new one.
+ *
+ * \param[in] fs The file system on which to operate.
+ * \param[in] cluster_num The cluster to which to append the new chain.
+ * \param[in] count The number of clusters to allocate.
+ * \returns 0 on failure, the number of the first new cluster on success.
+ */
+cluster_t fat_append_clusters(struct fat_fs_struct* fs, cluster_t cluster_num, cluster_t count)
+{
+ if(!fs)
+ return 0;
+
+ device_read_t device_read = fs->partition->device_read;
+ device_write_t device_write = fs->partition->device_write;
+ offset_t fat_offset = fs->header.fat_offset;
+ cluster_t count_left = count;
+ cluster_t cluster_current = fs->cluster_free;
+ cluster_t cluster_next = 0;
+ cluster_t cluster_count;
+ uint16_t fat_entry16;
+#if FAT_FAT32_SUPPORT
+ uint32_t fat_entry32;
+ uint8_t is_fat32 = (fs->partition->type == PARTITION_TYPE_FAT32);
+
+ if(is_fat32)
+ cluster_count = fs->header.fat_size / sizeof(fat_entry32);
+ else
+#endif
+ cluster_count = fs->header.fat_size / sizeof(fat_entry16);
+
+ fs->cluster_free = 0;
+ for(cluster_t cluster_left = cluster_count; cluster_left > 0; --cluster_left, ++cluster_current)
+ {
+ if(cluster_current < 2 || cluster_current >= cluster_count)
+ cluster_current = 2;
+
+#if FAT_FAT32_SUPPORT
+ if(is_fat32)
+ {
+ if(!device_read(fat_offset + (offset_t) cluster_current * sizeof(fat_entry32), (uint8_t*) &fat_entry32, sizeof(fat_entry32)))
+ return 0;
+ }
+ else
+#endif
+ {
+ if(!device_read(fat_offset + (offset_t) cluster_current * sizeof(fat_entry16), (uint8_t*) &fat_entry16, sizeof(fat_entry16)))
+ return 0;
+ }
+
+#if FAT_FAT32_SUPPORT
+ if(is_fat32)
+ {
+ /* check if this is a free cluster */
+ if(fat_entry32 != HTOL32(FAT32_CLUSTER_FREE))
+ continue;
+
+ /* If we don't need this free cluster for the
+ * current allocation, we keep it in mind for
+ * the next time.
+ */
+ if(count_left == 0)
+ {
+ fs->cluster_free = cluster_current;
+ break;
+ }
+
+ /* allocate cluster */
+ if(cluster_next == 0)
+ fat_entry32 = HTOL32(FAT32_CLUSTER_LAST_MAX);
+ else
+ fat_entry32 = htol32(cluster_next);
+
+ if(!device_write(fat_offset + (offset_t) cluster_current * sizeof(fat_entry32), (uint8_t*) &fat_entry32, sizeof(fat_entry32)))
+ break;
+ }
+ else
+#endif
+ {
+ /* check if this is a free cluster */
+ if(fat_entry16 != HTOL16(FAT16_CLUSTER_FREE))
+ continue;
+
+ /* If we don't need this free cluster for the
+ * current allocation, we keep it in mind for
+ * the next time.
+ */
+ if(count_left == 0)
+ {
+ fs->cluster_free = cluster_current;
+ break;
+ }
+
+ /* allocate cluster */
+ if(cluster_next == 0)
+ fat_entry16 = HTOL16(FAT16_CLUSTER_LAST_MAX);
+ else
+ fat_entry16 = htol16((uint16_t) cluster_next);
+
+ if(!device_write(fat_offset + (offset_t) cluster_current * sizeof(fat_entry16), (uint8_t*) &fat_entry16, sizeof(fat_entry16)))
+ break;
+ }
+
+ cluster_next = cluster_current;
+ --count_left;
+ }
+
+ do
+ {
+ if(count_left > 0)
+ break;
+
+ /* We allocated a new cluster chain. Now join
+ * it with the existing one (if any).
+ */
+ if(cluster_num >= 2)
+ {
+#if FAT_FAT32_SUPPORT
+ if(is_fat32)
+ {
+ fat_entry32 = htol32(cluster_next);
+
+ if(!device_write(fat_offset + (offset_t) cluster_num * sizeof(fat_entry32), (uint8_t*) &fat_entry32, sizeof(fat_entry32)))
+ break;
+ }
+ else
+#endif
+ {
+ fat_entry16 = htol16((uint16_t) cluster_next);
+
+ if(!device_write(fat_offset + (offset_t) cluster_num * sizeof(fat_entry16), (uint8_t*) &fat_entry16, sizeof(fat_entry16)))
+ break;
+ }
+ }
+
+ return cluster_next;
+
+ } while(0);
+
+ /* No space left on device or writing error.
+ * Free up all clusters already allocated.
+ */
+ fat_free_clusters(fs, cluster_next);
+
+ return 0;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Frees a cluster chain, or a part thereof.
+ *
+ * Marks the specified cluster and all clusters which are sequentially
+ * referenced by it as free. They may then be used again for future
+ * file allocations.
+ *
+ * \note If this function is used for freeing just a part of a cluster
+ * chain, the new end of the chain is not correctly terminated
+ * within the FAT. Use fat_terminate_clusters() instead.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] cluster_num The starting cluster of the chain which to free.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_terminate_clusters
+ */
+uint8_t fat_free_clusters(struct fat_fs_struct* fs, cluster_t cluster_num)
+{
+ if(!fs || cluster_num < 2)
+ return 0;
+
+ offset_t fat_offset = fs->header.fat_offset;
+#if FAT_FAT32_SUPPORT
+ if(fs->partition->type == PARTITION_TYPE_FAT32)
+ {
+ uint32_t fat_entry;
+ while(cluster_num)
+ {
+ if(!fs->partition->device_read(fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry)))
+ return 0;
+
+ /* get next cluster of current cluster before freeing current cluster */
+ uint32_t cluster_num_next = ltoh32(fat_entry);
+
+ if(cluster_num_next == FAT32_CLUSTER_FREE)
+ return 1;
+ if(cluster_num_next == FAT32_CLUSTER_BAD ||
+ (cluster_num_next >= FAT32_CLUSTER_RESERVED_MIN &&
+ cluster_num_next <= FAT32_CLUSTER_RESERVED_MAX
+ )
+ )
+ return 0;
+ if(cluster_num_next >= FAT32_CLUSTER_LAST_MIN && cluster_num_next <= FAT32_CLUSTER_LAST_MAX)
+ cluster_num_next = 0;
+
+ /* We know we will free the cluster, so remember it as
+ * free for the next allocation.
+ */
+ if(!fs->cluster_free)
+ fs->cluster_free = cluster_num;
+
+ /* free cluster */
+ fat_entry = HTOL32(FAT32_CLUSTER_FREE);
+ fs->partition->device_write(fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry));
+
+ /* We continue in any case here, even if freeing the cluster failed.
+ * The cluster is lost, but maybe we can still free up some later ones.
+ */
+
+ cluster_num = cluster_num_next;
+ }
+ }
+ else
+#endif
+ {
+ uint16_t fat_entry;
+ while(cluster_num)
+ {
+ if(!fs->partition->device_read(fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry)))
+ return 0;
+
+ /* get next cluster of current cluster before freeing current cluster */
+ uint16_t cluster_num_next = ltoh16(fat_entry);
+
+ if(cluster_num_next == FAT16_CLUSTER_FREE)
+ return 1;
+ if(cluster_num_next == FAT16_CLUSTER_BAD ||
+ (cluster_num_next >= FAT16_CLUSTER_RESERVED_MIN &&
+ cluster_num_next <= FAT16_CLUSTER_RESERVED_MAX
+ )
+ )
+ return 0;
+ if(cluster_num_next >= FAT16_CLUSTER_LAST_MIN && cluster_num_next <= FAT16_CLUSTER_LAST_MAX)
+ cluster_num_next = 0;
+
+ /* free cluster */
+ fat_entry = HTOL16(FAT16_CLUSTER_FREE);
+ fs->partition->device_write(fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry));
+
+ /* We continue in any case here, even if freeing the cluster failed.
+ * The cluster is lost, but maybe we can still free up some later ones.
+ */
+
+ cluster_num = cluster_num_next;
+ }
+ }
+
+ return 1;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Frees a part of a cluster chain and correctly terminates the rest.
+ *
+ * Marks the specified cluster as the new end of a cluster chain and
+ * frees all following clusters.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] cluster_num The new end of the cluster chain.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_free_clusters
+ */
+uint8_t fat_terminate_clusters(struct fat_fs_struct* fs, cluster_t cluster_num)
+{
+ if(!fs || cluster_num < 2)
+ return 0;
+
+ /* fetch next cluster before overwriting the cluster entry */
+ cluster_t cluster_num_next = fat_get_next_cluster(fs, cluster_num);
+
+ /* mark cluster as the last one */
+#if FAT_FAT32_SUPPORT
+ if(fs->partition->type == PARTITION_TYPE_FAT32)
+ {
+ uint32_t fat_entry = HTOL32(FAT32_CLUSTER_LAST_MAX);
+ if(!fs->partition->device_write(fs->header.fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry)))
+ return 0;
+ }
+ else
+#endif
+ {
+ uint16_t fat_entry = HTOL16(FAT16_CLUSTER_LAST_MAX);
+ if(!fs->partition->device_write(fs->header.fat_offset + (offset_t) cluster_num * sizeof(fat_entry), (uint8_t*) &fat_entry, sizeof(fat_entry)))
+ return 0;
+ }
+
+ /* free remaining clusters */
+ if(cluster_num_next)
+ return fat_free_clusters(fs, cluster_num_next);
+ else
+ return 1;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Clears a single cluster.
+ *
+ * The complete cluster is filled with zeros.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] cluster_num The cluster to clear.
+ * \returns 0 on failure, 1 on success.
+ */
+uint8_t fat_clear_cluster(const struct fat_fs_struct* fs, cluster_t cluster_num)
+{
+ if(cluster_num < 2)
+ return 0;
+
+ offset_t cluster_offset = fat_cluster_offset(fs, cluster_num);
+
+ uint8_t zero[16];
+ memset(zero, 0, sizeof(zero));
+ return fs->partition->device_write_interval(cluster_offset,
+ zero,
+ fs->header.cluster_size,
+ fat_clear_cluster_callback,
+ 0
+ );
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Callback function for clearing a cluster.
+ */
+uintptr_t fat_clear_cluster_callback(uint8_t* buffer, offset_t offset, void* p)
+{
+ (void)buffer;
+ (void)offset;
+ (void)p;
+ return 16;
+}
+#endif
+
+/**
+ * \ingroup fat_fs
+ * Calculates the offset of the specified cluster.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] cluster_num The cluster whose offset to calculate.
+ * \returns The cluster offset.
+ */
+offset_t fat_cluster_offset(const struct fat_fs_struct* fs, cluster_t cluster_num)
+{
+ if(!fs || cluster_num < 2)
+ return 0;
+
+ return fs->header.cluster_zero_offset + (offset_t) (cluster_num - 2) * fs->header.cluster_size;
+}
+
+/**
+ * \ingroup fat_file
+ * Retrieves the directory entry of a path.
+ *
+ * The given path may both describe a file or a directory.
+ *
+ * \param[in] fs The FAT filesystem on which to search.
+ * \param[in] path The path of which to read the directory entry.
+ * \param[out] dir_entry The directory entry to fill.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_read_dir
+ */
+uint8_t fat_get_dir_entry_of_path(struct fat_fs_struct* fs, const char* path, struct fat_dir_entry_struct* dir_entry)
+{
+ if(!fs || !path || path[0] == '\0' || !dir_entry)
+ return 0;
+
+ if(path[0] == '/')
+ ++path;
+
+ /* begin with the root directory */
+ memset(dir_entry, 0, sizeof(*dir_entry));
+ dir_entry->attributes = FAT_ATTRIB_DIR;
+
+ while(1)
+ {
+ if(path[0] == '\0')
+ return 1;
+
+ struct fat_dir_struct* dd = fat_open_dir(fs, dir_entry);
+ if(!dd)
+ break;
+
+ /* extract the next hierarchy we will search for */
+ const char* sub_path = strchr(path, '/');
+ uint8_t length_to_sep;
+ if(sub_path)
+ {
+ length_to_sep = sub_path - path;
+ ++sub_path;
+ }
+ else
+ {
+ length_to_sep = strlen(path);
+ sub_path = path + length_to_sep;
+ }
+
+ /* read directory entries */
+ while(fat_read_dir(dd, dir_entry))
+ {
+ /* check if we have found the next hierarchy */
+ if((strlen(dir_entry->long_name) != length_to_sep ||
+ strncmp(path, dir_entry->long_name, length_to_sep) != 0))
+ continue;
+
+ fat_close_dir(dd);
+ dd = 0;
+
+ if(path[length_to_sep] == '\0')
+ /* we iterated through the whole path and have found the file */
+ return 1;
+
+ if(dir_entry->attributes & FAT_ATTRIB_DIR)
+ {
+ /* we found a parent directory of the file we are searching for */
+ path = sub_path;
+ break;
+ }
+
+ /* a parent of the file exists, but not the file itself */
+ return 0;
+ }
+
+ fat_close_dir(dd);
+ }
+
+ return 0;
+}
+
+/**
+ * \ingroup fat_file
+ * Opens a file on a FAT filesystem.
+ *
+ * \param[in] fs The filesystem on which the file to open lies.
+ * \param[in] dir_entry The directory entry of the file to open.
+ * \returns The file handle, or 0 on failure.
+ * \see fat_close_file
+ */
+struct fat_file_struct* fat_open_file(struct fat_fs_struct* fs, const struct fat_dir_entry_struct* dir_entry)
+{
+ if(!fs || !dir_entry || (dir_entry->attributes & FAT_ATTRIB_DIR))
+ return 0;
+
+#if USE_DYNAMIC_MEMORY
+ struct fat_file_struct* fd = malloc(sizeof(*fd));
+ if(!fd)
+ return 0;
+#else
+ struct fat_file_struct* fd = fat_file_handles;
+ uint8_t i;
+ for(i = 0; i < FAT_FILE_COUNT; ++i)
+ {
+ if(!fd->fs)
+ break;
+
+ ++fd;
+ }
+ if(i >= FAT_FILE_COUNT)
+ return 0;
+#endif
+
+ memcpy(&fd->dir_entry, dir_entry, sizeof(*dir_entry));
+ fd->fs = fs;
+ fd->pos = 0;
+ fd->pos_cluster = dir_entry->cluster;
+
+ return fd;
+}
+
+/**
+ * \ingroup fat_file
+ * Closes a file.
+ *
+ * \param[in] fd The file handle of the file to close.
+ * \see fat_open_file
+ */
+void fat_close_file(struct fat_file_struct* fd)
+{
+ if(fd)
+ {
+#if FAT_DELAY_DIRENTRY_UPDATE
+ /* write directory entry */
+ fat_write_dir_entry(fd->fs, &fd->dir_entry);
+#endif
+
+#if USE_DYNAMIC_MEMORY
+ free(fd);
+#else
+ fd->fs = 0;
+#endif
+ }
+}
+
+/**
+ * \ingroup fat_file
+ * Reads data from a file.
+ *
+ * The data requested is read from the current file location.
+ *
+ * \param[in] fd The file handle of the file from which to read.
+ * \param[out] buffer The buffer into which to write.
+ * \param[in] buffer_len The amount of data to read.
+ * \returns The number of bytes read, 0 on end of file, or -1 on failure.
+ * \see fat_write_file
+ */
+intptr_t fat_read_file(struct fat_file_struct* fd, uint8_t* buffer, uintptr_t buffer_len)
+{
+ /* check arguments */
+ if(!fd || !buffer || buffer_len < 1)
+ return -1;
+
+ /* determine number of bytes to read */
+ if(fd->pos + buffer_len > fd->dir_entry.file_size)
+ buffer_len = fd->dir_entry.file_size - fd->pos;
+ if(buffer_len == 0)
+ return 0;
+
+ uint16_t cluster_size = fd->fs->header.cluster_size;
+ cluster_t cluster_num = fd->pos_cluster;
+ uintptr_t buffer_left = buffer_len;
+ uint16_t first_cluster_offset = (uint16_t) (fd->pos & (cluster_size - 1));
+
+ /* find cluster in which to start reading */
+ if(!cluster_num)
+ {
+ cluster_num = fd->dir_entry.cluster;
+
+ if(!cluster_num)
+ {
+ if(!fd->pos)
+ return 0;
+ else
+ return -1;
+ }
+
+ if(fd->pos)
+ {
+ uint32_t pos = fd->pos;
+ while(pos >= cluster_size)
+ {
+ pos -= cluster_size;
+ cluster_num = fat_get_next_cluster(fd->fs, cluster_num);
+ if(!cluster_num)
+ return -1;
+ }
+ }
+ }
+
+ /* read data */
+ do
+ {
+ /* calculate data size to copy from cluster */
+ offset_t cluster_offset = fat_cluster_offset(fd->fs, cluster_num) + first_cluster_offset;
+ uint16_t copy_length = cluster_size - first_cluster_offset;
+ if(copy_length > buffer_left)
+ copy_length = buffer_left;
+
+ /* read data */
+ if(!fd->fs->partition->device_read(cluster_offset, buffer, copy_length))
+ return buffer_len - buffer_left;
+
+ /* calculate new file position */
+ buffer += copy_length;
+ buffer_left -= copy_length;
+ fd->pos += copy_length;
+
+ if(first_cluster_offset + copy_length >= cluster_size)
+ {
+ /* we are on a cluster boundary, so get the next cluster */
+ if((cluster_num = fat_get_next_cluster(fd->fs, cluster_num)))
+ {
+ first_cluster_offset = 0;
+ }
+ else
+ {
+ fd->pos_cluster = 0;
+ return buffer_len - buffer_left;
+ }
+ }
+
+ fd->pos_cluster = cluster_num;
+
+ } while(buffer_left > 0); /* check if we are done */
+
+ return buffer_len;
+}
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_file
+ * Writes data to a file.
+ *
+ * The data is written to the current file location.
+ *
+ * \param[in] fd The file handle of the file to which to write.
+ * \param[in] buffer The buffer from which to read the data to be written.
+ * \param[in] buffer_len The amount of data to write.
+ * \returns The number of bytes written (0 or something less than \c buffer_len on disk full) or -1 on failure.
+ * \see fat_read_file
+ */
+intptr_t fat_write_file(struct fat_file_struct* fd, const uint8_t* buffer, uintptr_t buffer_len)
+{
+ /* check arguments */
+ if(!fd || !buffer || buffer_len < 1)
+ return -1;
+ if(fd->pos > fd->dir_entry.file_size)
+ return -1;
+
+ uint16_t cluster_size = fd->fs->header.cluster_size;
+ cluster_t cluster_num = fd->pos_cluster;
+ uintptr_t buffer_left = buffer_len;
+ uint16_t first_cluster_offset = (uint16_t) (fd->pos & (cluster_size - 1));
+
+ /* find cluster in which to start writing */
+ if(!cluster_num)
+ {
+ cluster_num = fd->dir_entry.cluster;
+
+ if(!cluster_num)
+ {
+ if(!fd->pos)
+ {
+ /* empty file */
+ fd->dir_entry.cluster = cluster_num = fat_append_clusters(fd->fs, 0, 1);
+ if(!cluster_num)
+ return 0;
+ }
+ else
+ {
+ return -1;
+ }
+ }
+
+ if(fd->pos)
+ {
+ uint32_t pos = fd->pos;
+ cluster_t cluster_num_next;
+ while(pos >= cluster_size)
+ {
+ pos -= cluster_size;
+ cluster_num_next = fat_get_next_cluster(fd->fs, cluster_num);
+ if(!cluster_num_next)
+ {
+ if(pos != 0)
+ return -1; /* current file position points beyond end of file */
+
+ /* the file exactly ends on a cluster boundary, and we append to it */
+ cluster_num_next = fat_append_clusters(fd->fs, cluster_num, 1);
+ if(!cluster_num_next)
+ return 0;
+ }
+
+ cluster_num = cluster_num_next;
+ }
+ }
+ }
+
+ /* write data */
+ do
+ {
+ /* calculate data size to write to cluster */
+ offset_t cluster_offset = fat_cluster_offset(fd->fs, cluster_num) + first_cluster_offset;
+ uint16_t write_length = cluster_size - first_cluster_offset;
+ if(write_length > buffer_left)
+ write_length = buffer_left;
+
+ /* write data which fits into the current cluster */
+ if(!fd->fs->partition->device_write(cluster_offset, buffer, write_length))
+ break;
+
+ /* calculate new file position */
+ buffer += write_length;
+ buffer_left -= write_length;
+ fd->pos += write_length;
+
+ if(first_cluster_offset + write_length >= cluster_size)
+ {
+ /* we are on a cluster boundary, so get the next cluster */
+ cluster_t cluster_num_next = fat_get_next_cluster(fd->fs, cluster_num);
+ if(!cluster_num_next && buffer_left > 0)
+ /* we reached the last cluster, append a new one */
+ cluster_num_next = fat_append_clusters(fd->fs, cluster_num, 1);
+ if(!cluster_num_next)
+ {
+ fd->pos_cluster = 0;
+ break;
+ }
+
+ cluster_num = cluster_num_next;
+ first_cluster_offset = 0;
+ }
+
+ fd->pos_cluster = cluster_num;
+
+ } while(buffer_left > 0); /* check if we are done */
+
+ /* update directory entry */
+ if(fd->pos > fd->dir_entry.file_size)
+ {
+#if !FAT_DELAY_DIRENTRY_UPDATE
+ uint32_t size_old = fd->dir_entry.file_size;
+#endif
+
+ /* update file size */
+ fd->dir_entry.file_size = fd->pos;
+
+#if !FAT_DELAY_DIRENTRY_UPDATE
+ /* write directory entry */
+ if(!fat_write_dir_entry(fd->fs, &fd->dir_entry))
+ {
+ /* We do not return an error here since we actually wrote
+ * some data to disk. So we calculate the amount of data
+ * we wrote to disk and which lies within the old file size.
+ */
+ buffer_left = fd->pos - size_old;
+ fd->pos = size_old;
+ }
+#endif
+ }
+
+ return buffer_len - buffer_left;
+}
+#endif
+
+/**
+ * \ingroup fat_file
+ * Repositions the read/write file offset.
+ *
+ * Changes the file offset where the next call to fat_read_file()
+ * or fat_write_file() starts reading/writing.
+ *
+ * If the new offset is beyond the end of the file, fat_resize_file()
+ * is implicitly called, i.e. the file is expanded.
+ *
+ * The new offset can be given in different ways determined by
+ * the \c whence parameter:
+ * - \b FAT_SEEK_SET: \c *offset is relative to the beginning of the file.
+ * - \b FAT_SEEK_CUR: \c *offset is relative to the current file position.
+ * - \b FAT_SEEK_END: \c *offset is relative to the end of the file.
+ *
+ * The resulting absolute offset is written to the location the \c offset
+ * parameter points to.
+ *
+ * Calling this function can also be used to retrieve the current file position:
+ \code
+ int32_t file_pos = 0;
+ if(!fat_seek_file(fd, &file_pos, FAT_SEEK_CUR))
+ {
+ // error
+ }
+ // file_pos now contains the absolute file position
+ \endcode
+ *
+ * \param[in] fd The file decriptor of the file on which to seek.
+ * \param[in,out] offset A pointer to the new offset, as affected by the \c whence
+ * parameter. The function writes the new absolute offset
+ * to this location before it returns.
+ * \param[in] whence Affects the way \c offset is interpreted, see above.
+ * \returns 0 on failure, 1 on success.
+ */
+uint8_t fat_seek_file(struct fat_file_struct* fd, int32_t* offset, uint8_t whence)
+{
+ if(!fd || !offset)
+ return 0;
+
+ uint32_t new_pos = fd->pos;
+ switch(whence)
+ {
+ case FAT_SEEK_SET:
+ new_pos = *offset;
+ break;
+ case FAT_SEEK_CUR:
+ new_pos += *offset;
+ break;
+ case FAT_SEEK_END:
+ new_pos = fd->dir_entry.file_size + *offset;
+ break;
+ default:
+ return 0;
+ }
+
+ if(new_pos > fd->dir_entry.file_size
+#if FAT_WRITE_SUPPORT
+ && !fat_resize_file(fd, new_pos)
+#endif
+ )
+ return 0;
+
+ fd->pos = new_pos;
+ fd->pos_cluster = 0;
+
+ *offset = (int32_t) new_pos;
+ return 1;
+}
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_file
+ * Resizes a file to have a specific size.
+ *
+ * Enlarges or shrinks the file pointed to by the file descriptor to have
+ * exactly the specified size.
+ *
+ * If the file is truncated, all bytes having an equal or larger offset
+ * than the given size are lost. If the file is expanded, the additional
+ * bytes are allocated.
+ *
+ * \note Please be aware that this function just allocates or deallocates disk
+ * space, it does not explicitely clear it. To avoid data leakage, this
+ * must be done manually.
+ *
+ * \param[in] fd The file decriptor of the file which to resize.
+ * \param[in] size The new size of the file.
+ * \returns 0 on failure, 1 on success.
+ */
+uint8_t fat_resize_file(struct fat_file_struct* fd, uint32_t size)
+{
+ if(!fd)
+ return 0;
+
+ cluster_t cluster_num = fd->dir_entry.cluster;
+ uint16_t cluster_size = fd->fs->header.cluster_size;
+ uint32_t size_new = size;
+
+ do
+ {
+ if(cluster_num == 0 && size_new == 0)
+ /* the file stays empty */
+ break;
+
+ /* seek to the next cluster as long as we need the space */
+ while(size_new > cluster_size)
+ {
+ /* get next cluster of file */
+ cluster_t cluster_num_next = fat_get_next_cluster(fd->fs, cluster_num);
+ if(cluster_num_next)
+ {
+ cluster_num = cluster_num_next;
+ size_new -= cluster_size;
+ }
+ else
+ {
+ break;
+ }
+ }
+
+ if(size_new > cluster_size || cluster_num == 0)
+ {
+ /* Allocate new cluster chain and append
+ * it to the existing one, if available.
+ */
+ cluster_t cluster_count = (size_new + cluster_size - 1) / cluster_size;
+ cluster_t cluster_new_chain = fat_append_clusters(fd->fs, cluster_num, cluster_count);
+ if(!cluster_new_chain)
+ return 0;
+
+ if(!cluster_num)
+ {
+ cluster_num = cluster_new_chain;
+ fd->dir_entry.cluster = cluster_num;
+ }
+ }
+
+ /* write new directory entry */
+ fd->dir_entry.file_size = size;
+ if(size == 0)
+ fd->dir_entry.cluster = 0;
+ if(!fat_write_dir_entry(fd->fs, &fd->dir_entry))
+ return 0;
+
+ if(size == 0)
+ {
+ /* free all clusters of file */
+ fat_free_clusters(fd->fs, cluster_num);
+ }
+ else if(size_new <= cluster_size)
+ {
+ /* free all clusters no longer needed */
+ fat_terminate_clusters(fd->fs, cluster_num);
+ }
+
+ } while(0);
+
+ /* correct file position */
+ if(size < fd->pos)
+ {
+ fd->pos = size;
+ fd->pos_cluster = 0;
+ }
+
+ return 1;
+}
+#endif
+
+/**
+ * \ingroup fat_dir
+ * Opens a directory.
+ *
+ * \param[in] fs The filesystem on which the directory to open resides.
+ * \param[in] dir_entry The directory entry which stands for the directory to open.
+ * \returns An opaque directory descriptor on success, 0 on failure.
+ * \see fat_close_dir
+ */
+struct fat_dir_struct* fat_open_dir(struct fat_fs_struct* fs, const struct fat_dir_entry_struct* dir_entry)
+{
+ if(!fs || !dir_entry || !(dir_entry->attributes & FAT_ATTRIB_DIR))
+ return 0;
+
+#if USE_DYNAMIC_MEMORY
+ struct fat_dir_struct* dd = malloc(sizeof(*dd));
+ if(!dd)
+ return 0;
+#else
+ struct fat_dir_struct* dd = fat_dir_handles;
+ uint8_t i;
+ for(i = 0; i < FAT_DIR_COUNT; ++i)
+ {
+ if(!dd->fs)
+ break;
+
+ ++dd;
+ }
+ if(i >= FAT_DIR_COUNT)
+ return 0;
+#endif
+
+ memcpy(&dd->dir_entry, dir_entry, sizeof(*dir_entry));
+ dd->fs = fs;
+ dd->entry_cluster = dir_entry->cluster;
+ dd->entry_offset = 0;
+
+ return dd;
+}
+
+/**
+ * \ingroup fat_dir
+ * Closes a directory descriptor.
+ *
+ * This function destroys a directory descriptor which was
+ * previously obtained by calling fat_open_dir(). When this
+ * function returns, the given descriptor will be invalid.
+ *
+ * \param[in] dd The directory descriptor to close.
+ * \see fat_open_dir
+ */
+void fat_close_dir(struct fat_dir_struct* dd)
+{
+ if(dd)
+#if USE_DYNAMIC_MEMORY
+ free(dd);
+#else
+ dd->fs = 0;
+#endif
+}
+
+/**
+ * \ingroup fat_dir
+ * Reads the next directory entry contained within a parent directory.
+ *
+ * \param[in] dd The descriptor of the parent directory from which to read the entry.
+ * \param[out] dir_entry Pointer to a buffer into which to write the directory entry information.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_reset_dir
+ */
+uint8_t fat_read_dir(struct fat_dir_struct* dd, struct fat_dir_entry_struct* dir_entry)
+{
+ if(!dd || !dir_entry)
+ return 0;
+
+ /* get current position of directory handle */
+ struct fat_fs_struct* fs = dd->fs;
+ const struct fat_header_struct* header = &fs->header;
+ uint16_t cluster_size = header->cluster_size;
+ cluster_t cluster_num = dd->entry_cluster;
+ uint16_t cluster_offset = dd->entry_offset;
+ struct fat_read_dir_callback_arg arg;
+
+ if(cluster_offset >= cluster_size)
+ {
+ /* The latest call hit the border of the last cluster in
+ * the chain, but it still returned a directory entry.
+ * So we now reset the handle and signal the caller the
+ * end of the listing.
+ */
+ fat_reset_dir(dd);
+ return 0;
+ }
+
+ /* reset callback arguments */
+ memset(&arg, 0, sizeof(arg));
+ memset(dir_entry, 0, sizeof(*dir_entry));
+ arg.dir_entry = dir_entry;
+
+ /* check if we read from the root directory */
+ if(cluster_num == 0)
+ {
+#if FAT_FAT32_SUPPORT
+ if(fs->partition->type == PARTITION_TYPE_FAT32)
+ cluster_num = header->root_dir_cluster;
+ else
+#endif
+ cluster_size = header->cluster_zero_offset - header->root_dir_offset;
+ }
+
+ /* read entries */
+ uint8_t buffer[32];
+ while(!arg.finished)
+ {
+ /* read directory entries up to the cluster border */
+ uint16_t cluster_left = cluster_size - cluster_offset;
+ offset_t pos = cluster_offset;
+ if(cluster_num == 0)
+ pos += header->root_dir_offset;
+ else
+ pos += fat_cluster_offset(fs, cluster_num);
+
+ arg.bytes_read = 0;
+ if(!fs->partition->device_read_interval(pos,
+ buffer,
+ sizeof(buffer),
+ cluster_left,
+ fat_dir_entry_read_callback,
+ &arg)
+ )
+ return 0;
+
+ cluster_offset += arg.bytes_read;
+
+ if(cluster_offset >= cluster_size)
+ {
+ /* we reached the cluster border and switch to the next cluster */
+
+ /* get number of next cluster */
+ if((cluster_num = fat_get_next_cluster(fs, cluster_num)) != 0)
+ {
+ cluster_offset = 0;
+ continue;
+ }
+
+ /* we are at the end of the cluster chain */
+ if(!arg.finished)
+ {
+ /* directory entry not found, reset directory handle */
+ fat_reset_dir(dd);
+ return 0;
+ }
+ else
+ {
+ /* The current execution of the function has been successful,
+ * so we can not signal an end of the directory listing to
+ * the caller, but must wait for the next call. So we keep an
+ * invalid cluster offset to mark this directory handle's
+ * traversal as finished.
+ */
+ }
+
+ break;
+ }
+ }
+
+ dd->entry_cluster = cluster_num;
+ dd->entry_offset = cluster_offset;
+
+ return arg.finished;
+}
+
+/**
+ * \ingroup fat_dir
+ * Resets a directory handle.
+ *
+ * Resets the directory handle such that reading restarts
+ * with the first directory entry.
+ *
+ * \param[in] dd The directory handle to reset.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_read_dir
+ */
+uint8_t fat_reset_dir(struct fat_dir_struct* dd)
+{
+ if(!dd)
+ return 0;
+
+ dd->entry_cluster = dd->dir_entry.cluster;
+ dd->entry_offset = 0;
+ return 1;
+}
+
+/**
+ * \ingroup fat_fs
+ * Callback function for reading a directory entry.
+ *
+ * Interprets a raw directory entry and puts the contained
+ * information into a fat_dir_entry_struct structure.
+ *
+ * For a single file there may exist multiple directory
+ * entries. All except the last one are lfn entries, which
+ * contain parts of the long filename. The last directory
+ * entry is a traditional 8.3 style one. It contains all
+ * other information like size, cluster, date and time.
+ *
+ * \param[in] buffer A pointer to 32 bytes of raw data.
+ * \param[in] offset The absolute offset of the raw data.
+ * \param[in,out] p An argument structure controlling operation.
+ * \returns 0 on failure or completion, 1 if reading has
+ * to be continued
+ */
+uint8_t fat_dir_entry_read_callback(uint8_t* buffer, offset_t offset, void* p)
+{
+ struct fat_read_dir_callback_arg* arg = p;
+ struct fat_dir_entry_struct* dir_entry = arg->dir_entry;
+
+ arg->bytes_read += 32;
+
+ /* skip deleted or empty entries */
+ if(buffer[0] == FAT_DIRENTRY_DELETED || !buffer[0])
+ {
+#if FAT_LFN_SUPPORT
+ arg->checksum = 0;
+#endif
+ return 1;
+ }
+
+#if !FAT_LFN_SUPPORT
+ /* skip lfn entries */
+ if(buffer[11] == 0x0f)
+ return 1;
+#endif
+
+ char* long_name = dir_entry->long_name;
+#if FAT_LFN_SUPPORT
+ if(buffer[11] == 0x0f)
+ {
+ /* checksum validation */
+ if(arg->checksum == 0 || arg->checksum != buffer[13])
+ {
+ /* reset directory entry */
+ memset(dir_entry, 0, sizeof(*dir_entry));
+
+ arg->checksum = buffer[13];
+ dir_entry->entry_offset = offset;
+ }
+
+ /* lfn supports unicode, but we do not, for now.
+ * So we assume pure ascii and read only every
+ * second byte.
+ */
+ uint16_t char_offset = ((buffer[0] & 0x3f) - 1) * 13;
+ const uint8_t char_mapping[] = { 1, 3, 5, 7, 9, 14, 16, 18, 20, 22, 24, 28, 30 };
+ for(uint8_t i = 0; i <= 12 && char_offset + i < sizeof(dir_entry->long_name) - 1; ++i)
+ long_name[char_offset + i] = buffer[char_mapping[i]];
+
+ return 1;
+ }
+ else
+#endif
+ {
+#if FAT_LFN_SUPPORT
+ /* if we do not have a long name or the previous lfn does not match, take the 8.3 name */
+ if(long_name[0] == '\0' || arg->checksum != fat_calc_83_checksum(buffer))
+#endif
+ {
+ /* reset directory entry */
+ memset(dir_entry, 0, sizeof(*dir_entry));
+ dir_entry->entry_offset = offset;
+
+ uint8_t i;
+ for(i = 0; i < 8; ++i)
+ {
+ if(buffer[i] == ' ')
+ break;
+ long_name[i] = buffer[i];
+
+ /* Windows NT and later versions do not store lfn entries
+ * for 8.3 names which have a lowercase basename, extension
+ * or both when everything else is uppercase. They use two
+ * extra bits to signal a lowercase basename or extension.
+ */
+ if((buffer[12] & 0x08) && buffer[i] >= 'A' && buffer[i] <= 'Z')
+ long_name[i] += 'a' - 'A';
+ }
+ if(long_name[0] == 0x05)
+ long_name[0] = (char) FAT_DIRENTRY_DELETED;
+
+ if(buffer[8] != ' ')
+ {
+ long_name[i++] = '.';
+
+ uint8_t j = 8;
+ for(; j < 11; ++j)
+ {
+ if(buffer[j] == ' ')
+ break;
+ long_name[i] = buffer[j];
+
+ /* See above for the lowercase 8.3 name handling of
+ * Windows NT and later.
+ */
+ if((buffer[12] & 0x10) && buffer[j] >= 'A' && buffer[j] <= 'Z')
+ long_name[i] += 'a' - 'A';
+
+ ++i;
+ }
+ }
+
+ long_name[i] = '\0';
+ }
+
+ /* extract properties of file and store them within the structure */
+ dir_entry->attributes = buffer[11];
+ dir_entry->cluster = read16(&buffer[26]);
+#if FAT_FAT32_SUPPORT
+ dir_entry->cluster |= ((cluster_t) read16(&buffer[20])) << 16;
+#endif
+ dir_entry->file_size = read32(&buffer[28]);
+
+#if FAT_DATETIME_SUPPORT
+ dir_entry->modification_time = read16(&buffer[22]);
+ dir_entry->modification_date = read16(&buffer[24]);
+#endif
+
+ arg->finished = 1;
+ return 0;
+ }
+}
+
+#if DOXYGEN || FAT_LFN_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Calculates the checksum for 8.3 names used within the
+ * corresponding lfn directory entries.
+ *
+ * \param[in] file_name_83 The 11-byte file name buffer.
+ * \returns The checksum of the given file name.
+ */
+uint8_t fat_calc_83_checksum(const uint8_t* file_name_83)
+{
+ uint8_t checksum = file_name_83[0];
+ for(uint8_t i = 1; i < 11; ++i)
+ checksum = ((checksum >> 1) | (checksum << 7)) + file_name_83[i];
+
+ return checksum;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Searches for space where to store a directory entry.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] parent The directory in which to search.
+ * \param[in] dir_entry The directory entry for which to search space.
+ * \returns 0 on failure, a device offset on success.
+ */
+offset_t fat_find_offset_for_dir_entry(struct fat_fs_struct* fs, const struct fat_dir_struct* parent, const struct fat_dir_entry_struct* dir_entry)
+{
+ if(!fs || !dir_entry)
+ return 0;
+
+ /* search for a place where to write the directory entry to disk */
+#if FAT_LFN_SUPPORT
+ uint8_t free_dir_entries_needed = (strlen(dir_entry->long_name) + 12) / 13 + 1;
+ uint8_t free_dir_entries_found = 0;
+#endif
+ cluster_t cluster_num = parent->dir_entry.cluster;
+ offset_t dir_entry_offset = 0;
+ offset_t offset = 0;
+ offset_t offset_to = 0;
+#if FAT_FAT32_SUPPORT
+ uint8_t is_fat32 = (fs->partition->type == PARTITION_TYPE_FAT32);
+#endif
+
+ if(cluster_num == 0)
+ {
+#if FAT_FAT32_SUPPORT
+ if(is_fat32)
+ {
+ cluster_num = fs->header.root_dir_cluster;
+ }
+ else
+#endif
+ {
+ /* we read/write from the root directory entry */
+ offset = fs->header.root_dir_offset;
+ offset_to = fs->header.cluster_zero_offset;
+ dir_entry_offset = offset;
+ }
+ }
+
+ while(1)
+ {
+ if(offset == offset_to)
+ {
+ if(cluster_num == 0)
+ /* We iterated through the whole root directory and
+ * could not find enough space for the directory entry.
+ */
+ return 0;
+
+ if(offset)
+ {
+ /* We reached a cluster boundary and have to
+ * switch to the next cluster.
+ */
+
+ cluster_t cluster_next = fat_get_next_cluster(fs, cluster_num);
+ if(!cluster_next)
+ {
+ cluster_next = fat_append_clusters(fs, cluster_num, 1);
+ if(!cluster_next)
+ return 0;
+
+ /* we appended a new cluster and know it is free */
+ dir_entry_offset = fs->header.cluster_zero_offset +
+ (offset_t) (cluster_next - 2) * fs->header.cluster_size;
+
+ /* clear cluster to avoid garbage directory entries */
+ fat_clear_cluster(fs, cluster_next);
+
+ break;
+ }
+ cluster_num = cluster_next;
+ }
+
+ offset = fat_cluster_offset(fs, cluster_num);
+ offset_to = offset + fs->header.cluster_size;
+ dir_entry_offset = offset;
+#if FAT_LFN_SUPPORT
+ free_dir_entries_found = 0;
+#endif
+ }
+
+ /* read next lfn or 8.3 entry */
+ uint8_t first_char;
+ if(!fs->partition->device_read(offset, &first_char, sizeof(first_char)))
+ return 0;
+
+ /* check if we found a free directory entry */
+ if(first_char == FAT_DIRENTRY_DELETED || !first_char)
+ {
+ /* check if we have the needed number of available entries */
+#if FAT_LFN_SUPPORT
+ ++free_dir_entries_found;
+ if(free_dir_entries_found >= free_dir_entries_needed)
+#endif
+ break;
+
+ offset += 32;
+ }
+ else
+ {
+ offset += 32;
+ dir_entry_offset = offset;
+#if FAT_LFN_SUPPORT
+ free_dir_entries_found = 0;
+#endif
+ }
+ }
+
+ return dir_entry_offset;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Writes a directory entry to disk.
+ *
+ * \note The file name is not checked for invalid characters.
+ *
+ * \note The generation of the short 8.3 file name is quite
+ * simple. The first eight characters are used for the filename.
+ * The extension, if any, is made up of the first three characters
+ * following the last dot within the long filename. If the
+ * filename (without the extension) is longer than eight characters,
+ * the lower byte of the cluster number replaces the last two
+ * characters to avoid name clashes. In any other case, it is your
+ * responsibility to avoid name clashes.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] dir_entry The directory entry to write.
+ * \returns 0 on failure, 1 on success.
+ */
+uint8_t fat_write_dir_entry(const struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry)
+{
+ if(!fs || !dir_entry)
+ return 0;
+
+#if FAT_DATETIME_SUPPORT
+ {
+ uint16_t year;
+ uint8_t month;
+ uint8_t day;
+ uint8_t hour;
+ uint8_t min;
+ uint8_t sec;
+
+ fat_get_datetime(&year, &month, &day, &hour, &min, &sec);
+ fat_set_file_modification_date(dir_entry, year, month, day);
+ fat_set_file_modification_time(dir_entry, hour, min, sec);
+ }
+#endif
+
+ device_write_t device_write = fs->partition->device_write;
+ offset_t offset = dir_entry->entry_offset;
+ const char* name = dir_entry->long_name;
+ uint8_t name_len = strlen(name);
+#if FAT_LFN_SUPPORT
+ uint8_t lfn_entry_count = (name_len + 12) / 13;
+#endif
+ uint8_t buffer[32];
+
+ /* write 8.3 entry */
+
+ /* generate 8.3 file name */
+ memset(&buffer[0], ' ', 11);
+ char* name_ext = strrchr(name, '.');
+ if(name_ext && *++name_ext)
+ {
+ uint8_t name_ext_len = strlen(name_ext);
+ name_len -= name_ext_len + 1;
+
+ if(name_ext_len > 3)
+#if FAT_LFN_SUPPORT
+ name_ext_len = 3;
+#else
+ return 0;
+#endif
+
+ memcpy(&buffer[8], name_ext, name_ext_len);
+ }
+
+ if(name_len <= 8)
+ {
+ memcpy(buffer, name, name_len);
+
+#if FAT_LFN_SUPPORT
+ /* For now, we create lfn entries for all files,
+ * except the "." and ".." directory references.
+ * This is to avoid difficulties with capitalization,
+ * as 8.3 filenames allow uppercase letters only.
+ *
+ * Theoretically it would be possible to leave
+ * the 8.3 entry alone if the basename and the
+ * extension have no mixed capitalization.
+ */
+ if(name[0] == '.' &&
+ ((name[1] == '.' && name[2] == '\0') ||
+ name[1] == '\0')
+ )
+ lfn_entry_count = 0;
+#endif
+ }
+ else
+ {
+#if FAT_LFN_SUPPORT
+ memcpy(buffer, name, 8);
+
+ /* Minimize 8.3 name clashes by appending
+ * the lower byte of the cluster number.
+ */
+ uint8_t num = dir_entry->cluster & 0xff;
+
+ buffer[6] = (num < 0xa0) ? ('0' + (num >> 4)) : ('a' + (num >> 4));
+ num &= 0x0f;
+ buffer[7] = (num < 0x0a) ? ('0' + num) : ('a' + num);
+#else
+ return 0;
+#endif
+ }
+ if(buffer[0] == FAT_DIRENTRY_DELETED)
+ buffer[0] = 0x05;
+
+ /* fill directory entry buffer */
+ memset(&buffer[11], 0, sizeof(buffer) - 11);
+ buffer[0x0b] = dir_entry->attributes;
+#if FAT_DATETIME_SUPPORT
+ write16(&buffer[0x16], dir_entry->modification_time);
+ write16(&buffer[0x18], dir_entry->modification_date);
+#endif
+#if FAT_FAT32_SUPPORT
+ write16(&buffer[0x14], (uint16_t) (dir_entry->cluster >> 16));
+#endif
+ write16(&buffer[0x1a], dir_entry->cluster);
+ write32(&buffer[0x1c], dir_entry->file_size);
+
+ /* write to disk */
+#if FAT_LFN_SUPPORT
+ if(!device_write(offset + (uint16_t) lfn_entry_count * 32, buffer, sizeof(buffer)))
+#else
+ if(!device_write(offset, buffer, sizeof(buffer)))
+#endif
+ return 0;
+
+#if FAT_LFN_SUPPORT
+ /* calculate checksum of 8.3 name */
+ uint8_t checksum = fat_calc_83_checksum(buffer);
+
+ /* write lfn entries */
+ for(uint8_t lfn_entry = lfn_entry_count; lfn_entry > 0; --lfn_entry)
+ {
+ memset(buffer, 0xff, sizeof(buffer));
+
+ /* set file name */
+ const char* long_name_curr = name + (lfn_entry - 1) * 13;
+ uint8_t i = 1;
+ while(i < 0x1f)
+ {
+ buffer[i++] = *long_name_curr;
+ buffer[i++] = 0;
+
+ switch(i)
+ {
+ case 0x0b:
+ i = 0x0e;
+ break;
+ case 0x1a:
+ i = 0x1c;
+ break;
+ }
+
+ if(!*long_name_curr++)
+ break;
+ }
+
+ /* set index of lfn entry */
+ buffer[0x00] = lfn_entry;
+ if(lfn_entry == lfn_entry_count)
+ buffer[0x00] |= FAT_DIRENTRY_LFNLAST;
+
+ /* mark as lfn entry */
+ buffer[0x0b] = 0x0f;
+
+ /* set 8.3 checksum */
+ buffer[0x0d] = checksum;
+
+ /* clear reserved bytes */
+ buffer[0x0c] = 0;
+ buffer[0x1a] = 0;
+ buffer[0x1b] = 0;
+
+ /* write entry */
+ device_write(offset, buffer, sizeof(buffer));
+
+ offset += sizeof(buffer);
+ }
+#endif
+
+ return 1;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_file
+ * Creates a file.
+ *
+ * Creates a file and obtains the directory entry of the
+ * new file. If the file to create already exists, the
+ * directory entry of the existing file will be returned
+ * within the dir_entry parameter.
+ *
+ * \note The file name is not checked for invalid characters.
+ *
+ * \note The generation of the short 8.3 file name is quite
+ * simple. The first eight characters are used for the filename.
+ * The extension, if any, is made up of the first three characters
+ * following the last dot within the long filename. If the
+ * filename (without the extension) is longer than eight characters,
+ * the lower byte of the cluster number replaces the last two
+ * characters to avoid name clashes. In any other case, it is your
+ * responsibility to avoid name clashes.
+ *
+ * \param[in] parent The handle of the directory in which to create the file.
+ * \param[in] file The name of the file to create.
+ * \param[out] dir_entry The directory entry to fill for the new (or existing) file.
+ * \returns 0 on failure, 1 on success, 2 if the file already existed.
+ * \see fat_delete_file
+ */
+uint8_t fat_create_file(struct fat_dir_struct* parent, const char* file, struct fat_dir_entry_struct* dir_entry)
+{
+ if(!parent || !file || !file[0] || !dir_entry)
+ return 0;
+
+ /* check if the file already exists */
+ while(1)
+ {
+ if(!fat_read_dir(parent, dir_entry))
+ break;
+
+ if(strcmp(file, dir_entry->long_name) == 0)
+ {
+ fat_reset_dir(parent);
+ return 2;
+ }
+ }
+
+ struct fat_fs_struct* fs = parent->fs;
+
+ /* prepare directory entry with values already known */
+ memset(dir_entry, 0, sizeof(*dir_entry));
+ strncpy(dir_entry->long_name, file, sizeof(dir_entry->long_name) - 1);
+
+ /* find place where to store directory entry */
+ if(!(dir_entry->entry_offset = fat_find_offset_for_dir_entry(fs, parent, dir_entry)))
+ return 0;
+
+ /* write directory entry to disk */
+ if(!fat_write_dir_entry(fs, dir_entry))
+ return 0;
+
+ return 1;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_file
+ * Deletes a file or directory.
+ *
+ * If a directory is deleted without first deleting its
+ * subdirectories and files, disk space occupied by these
+ * files will get wasted as there is no chance to release
+ * it and mark it as free.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] dir_entry The directory entry of the file to delete.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_create_file
+ */
+uint8_t fat_delete_file(struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry)
+{
+ if(!fs || !dir_entry)
+ return 0;
+
+ /* get offset of the file's directory entry */
+ offset_t dir_entry_offset = dir_entry->entry_offset;
+ if(!dir_entry_offset)
+ return 0;
+
+#if FAT_LFN_SUPPORT
+ uint8_t buffer[12];
+ while(1)
+ {
+ /* read directory entry */
+ if(!fs->partition->device_read(dir_entry_offset, buffer, sizeof(buffer)))
+ return 0;
+
+ /* mark the directory entry as deleted */
+ buffer[0] = FAT_DIRENTRY_DELETED;
+
+ /* write back entry */
+ if(!fs->partition->device_write(dir_entry_offset, buffer, sizeof(buffer)))
+ return 0;
+
+ /* check if we deleted the whole entry */
+ if(buffer[11] != 0x0f)
+ break;
+
+ dir_entry_offset += 32;
+ }
+#else
+ /* mark the directory entry as deleted */
+ uint8_t first_char = FAT_DIRENTRY_DELETED;
+ if(!fs->partition->device_write(dir_entry_offset, &first_char, 1))
+ return 0;
+#endif
+
+ /* We deleted the directory entry. The next thing to do is
+ * marking all occupied clusters as free.
+ */
+ return (dir_entry->cluster == 0 || fat_free_clusters(fs, dir_entry->cluster));
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_file
+ * Moves or renames a file.
+ *
+ * Changes a file's name, optionally moving it into another
+ * directory as well. Before calling this function, the
+ * target file name must not exist. Moving a file to a
+ * different filesystem (i.e. \a parent_new doesn't lie on
+ * \a fs) is not supported.
+ *
+ * After successfully renaming (and moving) the file, the
+ * given directory entry is updated such that it points to
+ * the file's new location.
+ *
+ * \note The notes which apply to fat_create_file() also
+ * apply to this function.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in,out] dir_entry The directory entry of the file to move.
+ * \param[in] parent_new The handle of the new parent directory of the file.
+ * \param[in] file_new The file's new name.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_create_file, fat_delete_file, fat_move_dir
+ */
+uint8_t fat_move_file(struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry, struct fat_dir_struct* parent_new, const char* file_new)
+{
+ if(!fs || !dir_entry || !parent_new || (file_new && !file_new[0]))
+ return 0;
+ if(fs != parent_new->fs)
+ return 0;
+
+ /* use existing file name if none has been specified */
+ if(!file_new)
+ file_new = dir_entry->long_name;
+
+ /* create file with new file name */
+ struct fat_dir_entry_struct dir_entry_new;
+ if(!fat_create_file(parent_new, file_new, &dir_entry_new))
+ return 0;
+
+ /* copy members of directory entry which do not change with rename */
+ dir_entry_new.attributes = dir_entry->attributes;
+#if FAT_DATETIME_SUPPORT
+ dir_entry_new.modification_time = dir_entry->modification_time;
+ dir_entry_new.modification_date = dir_entry->modification_date;
+#endif
+ dir_entry_new.cluster = dir_entry->cluster;
+ dir_entry_new.file_size = dir_entry->file_size;
+
+ /* make the new file name point to the old file's content */
+ if(!fat_write_dir_entry(fs, &dir_entry_new))
+ {
+ fat_delete_file(fs, &dir_entry_new);
+ return 0;
+ }
+
+ /* delete the old file, but not its clusters, which have already been remapped above */
+ dir_entry->cluster = 0;
+ if(!fat_delete_file(fs, dir_entry))
+ return 0;
+
+ *dir_entry = dir_entry_new;
+ return 1;
+}
+#endif
+
+#if DOXYGEN || FAT_WRITE_SUPPORT
+/**
+ * \ingroup fat_dir
+ * Creates a directory.
+ *
+ * Creates a directory and obtains its directory entry.
+ * If the directory to create already exists, its
+ * directory entry will be returned within the dir_entry
+ * parameter.
+ *
+ * \note The notes which apply to fat_create_file() also
+ * apply to this function.
+ *
+ * \param[in] parent The handle of the parent directory of the new directory.
+ * \param[in] dir The name of the directory to create.
+ * \param[out] dir_entry The directory entry to fill for the new directory.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_delete_dir
+ */
+uint8_t fat_create_dir(struct fat_dir_struct* parent, const char* dir, struct fat_dir_entry_struct* dir_entry)
+{
+ if(!parent || !dir || !dir[0] || !dir_entry)
+ return 0;
+
+ /* check if the file or directory already exists */
+ while(fat_read_dir(parent, dir_entry))
+ {
+ if(strcmp(dir, dir_entry->long_name) == 0)
+ {
+ fat_reset_dir(parent);
+ return 0;
+ }
+ }
+
+ struct fat_fs_struct* fs = parent->fs;
+
+ /* allocate cluster which will hold directory entries */
+ cluster_t dir_cluster = fat_append_clusters(fs, 0, 1);
+ if(!dir_cluster)
+ return 0;
+
+ /* clear cluster to prevent bogus directory entries */
+ fat_clear_cluster(fs, dir_cluster);
+
+ memset(dir_entry, 0, sizeof(*dir_entry));
+ dir_entry->attributes = FAT_ATTRIB_DIR;
+
+ /* create "." directory self reference */
+ dir_entry->entry_offset = fs->header.cluster_zero_offset +
+ (offset_t) (dir_cluster - 2) * fs->header.cluster_size;
+ dir_entry->long_name[0] = '.';
+ dir_entry->cluster = dir_cluster;
+ if(!fat_write_dir_entry(fs, dir_entry))
+ {
+ fat_free_clusters(fs, dir_cluster);
+ return 0;
+ }
+
+ /* create ".." parent directory reference */
+ dir_entry->entry_offset += 32;
+ dir_entry->long_name[1] = '.';
+ dir_entry->cluster = parent->dir_entry.cluster;
+ if(!fat_write_dir_entry(fs, dir_entry))
+ {
+ fat_free_clusters(fs, dir_cluster);
+ return 0;
+ }
+
+ /* fill directory entry */
+ strncpy(dir_entry->long_name, dir, sizeof(dir_entry->long_name) - 1);
+ dir_entry->cluster = dir_cluster;
+
+ /* find place where to store directory entry */
+ if(!(dir_entry->entry_offset = fat_find_offset_for_dir_entry(fs, parent, dir_entry)))
+ {
+ fat_free_clusters(fs, dir_cluster);
+ return 0;
+ }
+
+ /* write directory to disk */
+ if(!fat_write_dir_entry(fs, dir_entry))
+ {
+ fat_free_clusters(fs, dir_cluster);
+ return 0;
+ }
+
+ return 1;
+}
+#endif
+
+/**
+ * \ingroup fat_dir
+ * Deletes a directory.
+ *
+ * This is just a synonym for fat_delete_file().
+ * If a directory is deleted without first deleting its
+ * subdirectories and files, disk space occupied by these
+ * files will get wasted as there is no chance to release
+ * it and mark it as free.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in] dir_entry The directory entry of the directory to delete.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_create_dir
+ */
+#ifdef DOXYGEN
+uint8_t fat_delete_dir(struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry);
+#endif
+
+/**
+ * \ingroup fat_dir
+ * Moves or renames a directory.
+ *
+ * This is just a synonym for fat_move_file().
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \param[in,out] dir_entry The directory entry of the directory to move.
+ * \param[in] parent_new The handle of the new parent directory.
+ * \param[in] dir_new The directory's new name.
+ * \returns 0 on failure, 1 on success.
+ * \see fat_create_dir, fat_delete_dir, fat_move_file
+ */
+#ifdef DOXYGEN
+uint8_t fat_move_dir(struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry, struct fat_dir_struct* parent_new, const char* dir_new);
+#endif
+
+#if DOXYGEN || FAT_DATETIME_SUPPORT
+/**
+ * \ingroup fat_file
+ * Returns the modification date of a file.
+ *
+ * \param[in] dir_entry The directory entry of which to return the modification date.
+ * \param[out] year The year the file was last modified.
+ * \param[out] month The month the file was last modified.
+ * \param[out] day The day the file was last modified.
+ */
+void fat_get_file_modification_date(const struct fat_dir_entry_struct* dir_entry, uint16_t* year, uint8_t* month, uint8_t* day)
+{
+ if(!dir_entry)
+ return;
+
+ *year = 1980 + ((dir_entry->modification_date >> 9) & 0x7f);
+ *month = (dir_entry->modification_date >> 5) & 0x0f;
+ *day = (dir_entry->modification_date >> 0) & 0x1f;
+}
+#endif
+
+#if DOXYGEN || FAT_DATETIME_SUPPORT
+/**
+ * \ingroup fat_file
+ * Returns the modification time of a file.
+ *
+ * \param[in] dir_entry The directory entry of which to return the modification time.
+ * \param[out] hour The hour the file was last modified.
+ * \param[out] min The min the file was last modified.
+ * \param[out] sec The sec the file was last modified.
+ */
+void fat_get_file_modification_time(const struct fat_dir_entry_struct* dir_entry, uint8_t* hour, uint8_t* min, uint8_t* sec)
+{
+ if(!dir_entry)
+ return;
+
+ *hour = (dir_entry->modification_time >> 11) & 0x1f;
+ *min = (dir_entry->modification_time >> 5) & 0x3f;
+ *sec = ((dir_entry->modification_time >> 0) & 0x1f) * 2;
+}
+#endif
+
+#if DOXYGEN || (FAT_WRITE_SUPPORT && FAT_DATETIME_SUPPORT)
+/**
+ * \ingroup fat_file
+ * Sets the modification time of a date.
+ *
+ * \param[in] dir_entry The directory entry for which to set the modification date.
+ * \param[in] year The year the file was last modified.
+ * \param[in] month The month the file was last modified.
+ * \param[in] day The day the file was last modified.
+ */
+void fat_set_file_modification_date(struct fat_dir_entry_struct* dir_entry, uint16_t year, uint8_t month, uint8_t day)
+{
+ if(!dir_entry)
+ return;
+
+ dir_entry->modification_date =
+ ((year - 1980) << 9) |
+ ((uint16_t) month << 5) |
+ ((uint16_t) day << 0);
+}
+#endif
+
+#if DOXYGEN || (FAT_WRITE_SUPPORT && FAT_DATETIME_SUPPORT)
+/**
+ * \ingroup fat_file
+ * Sets the modification time of a file.
+ *
+ * \param[in] dir_entry The directory entry for which to set the modification time.
+ * \param[in] hour The year the file was last modified.
+ * \param[in] min The month the file was last modified.
+ * \param[in] sec The day the file was last modified.
+ */
+void fat_set_file_modification_time(struct fat_dir_entry_struct* dir_entry, uint8_t hour, uint8_t min, uint8_t sec)
+{
+ if(!dir_entry)
+ return;
+
+ dir_entry->modification_time =
+ ((uint16_t) hour << 11) |
+ ((uint16_t) min << 5) |
+ ((uint16_t) sec >> 1) ;
+}
+#endif
+
+/**
+ * \ingroup fat_fs
+ * Returns the amount of total storage capacity of the filesystem in bytes.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \returns 0 on failure, the filesystem size in bytes otherwise.
+ */
+offset_t fat_get_fs_size(const struct fat_fs_struct* fs)
+{
+ if(!fs)
+ return 0;
+
+#if FAT_FAT32_SUPPORT
+ if(fs->partition->type == PARTITION_TYPE_FAT32)
+ return (offset_t) (fs->header.fat_size / 4 - 2) * fs->header.cluster_size;
+ else
+#endif
+ return (offset_t) (fs->header.fat_size / 2 - 2) * fs->header.cluster_size;
+}
+
+/**
+ * \ingroup fat_fs
+ * Returns the amount of free storage capacity on the filesystem in bytes.
+ *
+ * \note As the FAT filesystem is cluster based, this function does not
+ * return continuous values but multiples of the cluster size.
+ *
+ * \param[in] fs The filesystem on which to operate.
+ * \returns 0 on failure, the free filesystem space in bytes otherwise.
+ */
+offset_t fat_get_fs_free(const struct fat_fs_struct* fs)
+{
+ if(!fs)
+ return 0;
+
+ uint8_t fat[32];
+ struct fat_usage_count_callback_arg count_arg;
+ count_arg.cluster_count = 0;
+ count_arg.buffer_size = sizeof(fat);
+
+ offset_t fat_offset = fs->header.fat_offset;
+ uint32_t fat_size = fs->header.fat_size;
+ while(fat_size > 0)
+ {
+ uintptr_t length = UINTPTR_MAX - 1;
+ if(fat_size < length)
+ length = fat_size;
+
+ if(!fs->partition->device_read_interval(fat_offset,
+ fat,
+ sizeof(fat),
+ length,
+#if FAT_FAT32_SUPPORT
+ (fs->partition->type == PARTITION_TYPE_FAT16) ?
+ fat_get_fs_free_16_callback :
+ fat_get_fs_free_32_callback,
+#else
+ fat_get_fs_free_16_callback,
+#endif
+ &count_arg
+ )
+ )
+ return 0;
+
+ fat_offset += length;
+ fat_size -= length;
+ }
+
+ return (offset_t) count_arg.cluster_count * fs->header.cluster_size;
+}
+
+/**
+ * \ingroup fat_fs
+ * Callback function used for counting free clusters in a FAT.
+ */
+uint8_t fat_get_fs_free_16_callback(uint8_t* buffer, offset_t offset, void* p)
+{
+ struct fat_usage_count_callback_arg* count_arg = (struct fat_usage_count_callback_arg*) p;
+ uintptr_t buffer_size = count_arg->buffer_size;
+
+ (void)offset;
+
+ for(uintptr_t i = 0; i < buffer_size; i += 2, buffer += 2)
+ {
+ uint16_t cluster = read16(buffer);
+ if(cluster == HTOL16(FAT16_CLUSTER_FREE))
+ ++(count_arg->cluster_count);
+ }
+
+ return 1;
+}
+
+#if DOXYGEN || FAT_FAT32_SUPPORT
+/**
+ * \ingroup fat_fs
+ * Callback function used for counting free clusters in a FAT32.
+ */
+uint8_t fat_get_fs_free_32_callback(uint8_t* buffer, offset_t offset, void* p)
+{
+ struct fat_usage_count_callback_arg* count_arg = (struct fat_usage_count_callback_arg*) p;
+ uintptr_t buffer_size = count_arg->buffer_size;
+
+ (void)offset;
+
+ for(uintptr_t i = 0; i < buffer_size; i += 4, buffer += 4)
+ {
+ uint32_t cluster = read32(buffer);
+ if(cluster == HTOL32(FAT32_CLUSTER_FREE))
+ ++(count_arg->cluster_count);
+ }
+
+ return 1;
+}
+#endif
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FAT_H
+#define FAT_H
+
+#include <stdint.h>
+#include "fat_config.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup fat
+ *
+ * @{
+ */
+/**
+ * \file
+ * FAT header (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+/**
+ * \addtogroup fat_file
+ * @{
+ */
+
+/** The file is read-only. */
+#define FAT_ATTRIB_READONLY (1 << 0)
+/** The file is hidden. */
+#define FAT_ATTRIB_HIDDEN (1 << 1)
+/** The file is a system file. */
+#define FAT_ATTRIB_SYSTEM (1 << 2)
+/** The file is empty and has the volume label as its name. */
+#define FAT_ATTRIB_VOLUME (1 << 3)
+/** The file is a directory. */
+#define FAT_ATTRIB_DIR (1 << 4)
+/** The file has to be archived. */
+#define FAT_ATTRIB_ARCHIVE (1 << 5)
+
+/** The given offset is relative to the beginning of the file. */
+#define FAT_SEEK_SET 0
+/** The given offset is relative to the current read/write position. */
+#define FAT_SEEK_CUR 1
+/** The given offset is relative to the end of the file. */
+#define FAT_SEEK_END 2
+
+/**
+ * @}
+ */
+
+struct partition_struct;
+struct fat_fs_struct;
+struct fat_file_struct;
+struct fat_dir_struct;
+
+/**
+ * \ingroup fat_file
+ * Describes a directory entry.
+ */
+struct fat_dir_entry_struct
+{
+ /** The file's name, truncated to 31 characters. */
+ char long_name[32];
+ /** The file's attributes. Mask of the FAT_ATTRIB_* constants. */
+ uint8_t attributes;
+#if FAT_DATETIME_SUPPORT
+ /** Compressed representation of modification time. */
+ uint16_t modification_time;
+ /** Compressed representation of modification date. */
+ uint16_t modification_date;
+#endif
+ /** The cluster in which the file's first byte resides. */
+ cluster_t cluster;
+ /** The file's size. */
+ uint32_t file_size;
+ /** The total disk offset of this directory entry. */
+ offset_t entry_offset;
+};
+
+struct fat_fs_struct* fat_open(struct partition_struct* partition);
+void fat_close(struct fat_fs_struct* fs);
+
+struct fat_file_struct* fat_open_file(struct fat_fs_struct* fs, const struct fat_dir_entry_struct* dir_entry);
+void fat_close_file(struct fat_file_struct* fd);
+intptr_t fat_read_file(struct fat_file_struct* fd, uint8_t* buffer, uintptr_t buffer_len);
+intptr_t fat_write_file(struct fat_file_struct* fd, const uint8_t* buffer, uintptr_t buffer_len);
+uint8_t fat_seek_file(struct fat_file_struct* fd, int32_t* offset, uint8_t whence);
+uint8_t fat_resize_file(struct fat_file_struct* fd, uint32_t size);
+
+struct fat_dir_struct* fat_open_dir(struct fat_fs_struct* fs, const struct fat_dir_entry_struct* dir_entry);
+void fat_close_dir(struct fat_dir_struct* dd);
+uint8_t fat_read_dir(struct fat_dir_struct* dd, struct fat_dir_entry_struct* dir_entry);
+uint8_t fat_reset_dir(struct fat_dir_struct* dd);
+
+uint8_t fat_create_file(struct fat_dir_struct* parent, const char* file, struct fat_dir_entry_struct* dir_entry);
+uint8_t fat_delete_file(struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry);
+uint8_t fat_move_file(struct fat_fs_struct* fs, struct fat_dir_entry_struct* dir_entry, struct fat_dir_struct* parent_new, const char* file_new);
+uint8_t fat_create_dir(struct fat_dir_struct* parent, const char* dir, struct fat_dir_entry_struct* dir_entry);
+#define fat_delete_dir fat_delete_file
+#define fat_move_dir fat_move_file
+
+void fat_get_file_modification_date(const struct fat_dir_entry_struct* dir_entry, uint16_t* year, uint8_t* month, uint8_t* day);
+void fat_get_file_modification_time(const struct fat_dir_entry_struct* dir_entry, uint8_t* hour, uint8_t* min, uint8_t* sec);
+
+uint8_t fat_get_dir_entry_of_path(struct fat_fs_struct* fs, const char* path, struct fat_dir_entry_struct* dir_entry);
+
+offset_t fat_get_fs_size(const struct fat_fs_struct* fs);
+offset_t fat_get_fs_free(const struct fat_fs_struct* fs);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef FAT_CONFIG_H
+#define FAT_CONFIG_H
+
+#include <stdint.h>
+#include "sd_raw_config.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup fat
+ *
+ * @{
+ */
+/**
+ * \file
+ * FAT configuration (license: GPLv2 or LGPLv2.1)
+ */
+
+/**
+ * \ingroup fat_config
+ * Controls FAT write support.
+ *
+ * Set to 1 to enable FAT write support, set to 0 to disable it.
+ */
+#define FAT_WRITE_SUPPORT SD_RAW_WRITE_SUPPORT
+
+/**
+ * \ingroup fat_config
+ * Controls FAT long filename (LFN) support.
+ *
+ * Set to 1 to enable LFN support, set to 0 to disable it.
+ */
+#define FAT_LFN_SUPPORT 1
+
+/**
+ * \ingroup fat_config
+ * Controls FAT date and time support.
+ *
+ * Set to 1 to enable FAT date and time stamping support.
+ */
+#define FAT_DATETIME_SUPPORT 0
+
+/**
+ * \ingroup fat_config
+ * Controls FAT32 support.
+ *
+ * Set to 1 to enable FAT32 support.
+ */
+#define FAT_FAT32_SUPPORT SD_RAW_SDHC
+
+/**
+ * \ingroup fat_config
+ * Controls updates of directory entries.
+ *
+ * Set to 1 to delay directory entry updates until the file is closed.
+ * This can boost performance significantly, but may cause data loss
+ * if the file is not properly closed.
+ */
+#define FAT_DELAY_DIRENTRY_UPDATE 0
+
+/**
+ * \ingroup fat_config
+ * Determines the function used for retrieving current date and time.
+ *
+ * Define this to the function call which shall be used to retrieve
+ * current date and time.
+ *
+ * \note Used only when FAT_DATETIME_SUPPORT is 1.
+ *
+ * \param[out] year Pointer to a \c uint16_t which receives the current year.
+ * \param[out] month Pointer to a \c uint8_t which receives the current month.
+ * \param[out] day Pointer to a \c uint8_t which receives the current day.
+ * \param[out] hour Pointer to a \c uint8_t which receives the current hour.
+ * \param[out] min Pointer to a \c uint8_t which receives the current minute.
+ * \param[out] sec Pointer to a \c uint8_t which receives the current sec.
+ */
+#define fat_get_datetime(year, month, day, hour, min, sec) \
+ get_datetime(year, month, day, hour, min, sec)
+/* forward declaration for the above */
+void get_datetime(uint16_t* year, uint8_t* month, uint8_t* day, uint8_t* hour, uint8_t* min, uint8_t* sec);
+
+/**
+ * \ingroup fat_config
+ * Maximum number of filesystem handles.
+ */
+#define FAT_FS_COUNT 1
+
+/**
+ * \ingroup fat_config
+ * Maximum number of file handles.
+ */
+#define FAT_FILE_COUNT 1
+
+/**
+ * \ingroup fat_config
+ * Maximum number of directory handles.
+ */
+#define FAT_DIR_COUNT 2
+
+/**
+ * @}
+ */
+
+#if FAT_FAT32_SUPPORT
+ typedef uint32_t cluster_t;
+#else
+ typedef uint16_t cluster_t;
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/endian.h>
+#include <aversive/wait.h>
+#include <callout.h>
+#include <uart.h>
+
+#include "main.h"
+#include "gps_venus.h"
+#include "fat.h"
+#include "fat_config.h"
+#include "partition.h"
+#include "sd_raw.h"
+#include "sd_raw_config.h"
+#include "sd_log.h"
+
+#define debug_printf(args...) do { } while (0)
+/* #define debug_printf(args...) printf(args) */
+
+#define GPS_UART 0
+
+/*
+ https://code.google.com/p/hardware-ntp/source/browse/trunk/pcb-1/venus634.c?r=12
+ */
+
+/* some useful defines */
+#define START1 0xA0
+#define START2 0xA1
+#define END1 0x0D
+#define END2 0x0A
+
+/* proccessing states */
+
+#define S_RESET 0
+#define S_POSTRESET 1
+#define S_START1 2
+#define S_START2 3
+#define S_LENGTH1 4
+#define S_LENGTH2 5
+#define S_DISCARD 6
+#define S_COPY 7
+#define S_ENDCOPY 8
+#define S_END 9
+#define S_END1 10
+
+#define MAX_LEN 0x200
+struct gps_frame {
+ uint16_t len;
+ uint16_t cur_len;
+ uint8_t data[MAX_LEN];
+};
+
+static struct gps_frame rxframe;
+
+static struct gps_pos gps_pos;
+
+static struct callout gps_timer;
+
+/* INPUT FORMATS */
+
+/* packet types */
+/* 0x01 - System Restart */
+#define M_RESTART 0x01
+typedef struct {
+ uint8_t start_mode; /* 01 = Hot; 02 = Warm; 03 = Cold */
+ uint16_t utc_year; /* >= 1980 */
+ uint8_t utc_month;
+ uint8_t utc_day;
+ uint8_t utc_hour;
+ uint8_t utc_minute;
+ uint8_t utc_second;
+ int16_t latitude; /* -9000 to 9000, 1/100th degree, positive north */
+ int16_t longitude; /* -18000 to 18000, 1/100th degree, positive east */
+ int16_t altitude; /* -1000 to 18300, meters */
+} m_restart_t;
+
+
+/* 0x02 - Query Software Version */
+#define M_QUERYVER 0x02
+typedef struct {
+ uint8_t software_type; /* 01 = System Code */
+} m_queryver_t;
+
+
+/* 0x05 - Serial Port Configuration */
+#define M_SERCONF 0x05
+typedef struct {
+ uint8_t port; /* 00 = COM1 (the only port) */
+ uint8_t baud; /* 00 = 4800; 01 = 9600; 02 = 19200; 03 = 38400;
+ 04 = 57600; 05 = 115200; */
+ uint8_t update; /* 00 = SRAM only; 01 = SRAM and FLASH */
+} m_serconf_t;
+
+
+
+/* 0x08 - nmea interval */
+#define M_NMEAINTERVAL 0x08
+typedef struct {
+ uint8_t gga;
+ uint8_t gsa;
+ uint8_t gsv;
+ uint8_t gll;
+ uint8_t rmc;
+ uint8_t vtg;
+ uint8_t zda;
+ uint8_t attributes; /*
+ 0 - sram
+ 1 - flash
+ */
+
+} m_nmeainterval_t;
+
+
+/* 0x09 - Query Software Version */
+#define M_OUTPUT 0x09
+typedef struct {
+ uint8_t msg_type; /*
+ 00 = no output
+ 01 = nmea output
+ 02 = binary output
+ */
+} m_output_t;
+
+
+/* 0x0E - Update Rage */
+#define M_RATE 0x0E
+typedef struct {
+ uint8_t rate; /* Hz */
+ uint8_t attributes; /*
+ 0 - update ram
+ 1 - update ram & flash
+ */
+} m_rate_t;
+
+
+
+/* 0x37 - configure waas */
+#define M_WAAS 0x37
+typedef struct {
+ uint8_t enable; /* 01 = enable */
+ uint8_t attributes;
+} m_waas_t;
+
+/* 0x3C - nav mode */
+#define M_NAVMODE 0x3c
+typedef struct {
+ uint8_t navmode; /* 00 = car
+ 01 = pedestrian
+ */
+ uint8_t attributes;
+} m_navmode_t;
+
+
+#define DBG_SEND
+#define DBG_RECV
+
+void serial1_tx_cout(uint8_t c)
+{
+ debug_printf("%.2X ", c);
+ uart_send(GPS_UART, c);
+}
+
+void venus634_send(uint8_t type, void *payload, uint16_t len)
+{
+ uint8_t crc = 0, n;
+
+ debug_printf("SEND ");
+
+ /* now send the message */
+ /* header */
+ serial1_tx_cout(START1);
+ serial1_tx_cout(START2);
+ serial1_tx_cout(((len+1) >> 8) & 0xff);
+ serial1_tx_cout((len+1) & 0xff);
+ /* type and payload */
+ serial1_tx_cout(type);
+ crc ^= type;
+ for (n = 0; n < len; n++) {
+ serial1_tx_cout(*(uint8_t *)payload);
+ crc ^= *(uint8_t *)payload;
+ payload++;
+ }
+ /* checksum and tail */
+ serial1_tx_cout(crc);
+ serial1_tx_cout(END1);
+ serial1_tx_cout(END2);
+
+ debug_printf("\n");
+}
+
+void venus634_restart(void)
+{
+ m_restart_t restart;
+
+ memset(&restart,0,sizeof(m_restart_t));
+ restart.start_mode = 0x03; /* COLD */
+
+ venus634_send(M_RESTART,&restart,sizeof(m_restart_t));
+
+ return;
+}
+
+void venus634_config_serial(void)
+{
+ m_serconf_t serconf;
+
+ memset(&serconf,0,sizeof(m_serconf_t));
+ serconf.port = 0;
+ serconf.baud = 4;
+ serconf.update = 1;
+
+ venus634_send(M_SERCONF,&serconf,sizeof(m_serconf_t));
+
+ return;
+}
+
+
+void venus634_msg_type(void)
+{
+ m_output_t output;
+
+ memset(&output,0,sizeof(m_output_t));
+ output.msg_type = 0x02; /* binary msg */
+
+ venus634_send(M_OUTPUT,&output,sizeof(m_output_t));
+
+ return;
+}
+
+
+void venus634_rate(void)
+{
+ m_rate_t rate;
+
+ memset(&rate,0,sizeof(m_rate_t));
+ rate.rate = 20;
+ rate.attributes = 0; /* ram */
+
+ venus634_send(M_RATE,&rate,sizeof(m_rate_t));
+
+ return;
+}
+
+/* Wide Area Augmentation System: use ground stations to increase the precision
+ * of gps position */
+void venus634_waas(void)
+{
+ m_waas_t waas;
+
+ memset(&waas,0,sizeof(m_waas_t));
+ waas.enable = 1;
+ waas.attributes = 0; /* ram */
+
+ venus634_send(M_WAAS,&waas,sizeof(m_waas_t));
+
+ return;
+}
+
+/* Tell we are a car instead of a pedestrian */
+void venus634_navmode(void)
+{
+ m_navmode_t navmode;
+
+ memset(&navmode,0,sizeof(m_navmode_t));
+ navmode.navmode = 0; /* car */
+ navmode.attributes = 0; /* ram */
+
+ venus634_send(M_NAVMODE,&navmode,sizeof(m_navmode_t));
+
+ return;
+}
+
+/* frequency of NMEA messages */
+void venus634_nmea_interval(void)
+{
+ m_nmeainterval_t nmeainterval;
+
+ memset(&nmeainterval,0,sizeof(m_nmeainterval_t));
+
+ /* set frequency for nmea: 1 = once every one position fix, 2 = once
+ * every two position fix, ... */
+ nmeainterval.gga = 1; /* GPGGA interval - GPS Fix Data*/
+ nmeainterval.gsa = 1; /* GNSS DOPS and Active Satellites */
+ nmeainterval.gsv = 1; /* GNSS Satellites in View */
+ nmeainterval.gll = 1; /* Geographic Position - Latitude longitude */
+ nmeainterval.rmc = 1; /* Recomended Minimum Specific GNSS Sentence */
+ nmeainterval.vtg = 1; /* Course Over Ground and Ground Speed */
+ nmeainterval.zda = 1; /* Time & Date*/
+
+ nmeainterval.attributes = 1; /* ram flash */
+
+ venus634_send(M_NMEAINTERVAL,&nmeainterval,sizeof(m_nmeainterval_t));
+
+ return;
+}
+
+int8_t recv_cb(uint8_t byte)
+{
+ uint16_t i;
+
+ /* bytes 0 and 1 are start bytes */
+ if (rxframe.cur_len == 0) {
+ if (byte != START1) {
+ debug_printf("bad start1 %.2X\n", byte);
+ goto reset_buf;
+ }
+ }
+ else if (rxframe.cur_len == 1) {
+ if (byte != START2) {
+ debug_printf("bad start2 %.2X\n", byte);
+ goto reset_buf;
+ }
+ }
+ /* bytes 2 and 3 are the length of frame in network order */
+ else if (rxframe.cur_len == 2) {
+ rxframe.len = (uint16_t)byte << 8;
+ }
+ else if (rxframe.cur_len == 3) {
+ rxframe.len |= (uint16_t)byte;
+ if (rxframe.len > MAX_LEN) {
+ debug_printf("bad len %d\n", rxframe.len);
+ goto reset_buf;
+ }
+ }
+ /* next bytes are data (the 4 below is the size of header) */
+ else if ((rxframe.cur_len - 4) < rxframe.len) {
+ rxframe.data[rxframe.cur_len - 4] = byte;
+ }
+ /* then it's the crc */
+ else if ((rxframe.cur_len - 4) == rxframe.len) {
+ uint8_t crc = 0;
+
+ for (i = 0; i < rxframe.len; i++)
+ crc ^= rxframe.data[i];
+ if (byte != crc) {
+ debug_printf("invalid crc\n");
+ goto reset_buf;
+ }
+
+ }
+ /* and the last 2 bytes are stop bytes */
+ else if ((rxframe.cur_len - 4) == (rxframe.len + 1)) {
+ if (byte != END1) {
+ debug_printf("bad end1 %.2X\n", byte);
+ goto reset_buf;
+ }
+ }
+ else if ((rxframe.cur_len - 4) == (rxframe.len + 2)) {
+ if (byte != END2) {
+ debug_printf("bad end2 %.2X\n", byte);
+ goto reset_buf;
+ }
+ debug_printf("valid frame received\n");
+ rxframe.cur_len = 0;
+ return 0;
+ }
+ else /* should not happen */
+ goto reset_buf;
+
+ rxframe.cur_len ++;
+ return 1;
+
+ reset_buf:
+ rxframe.cur_len = 0;
+ return 1;
+}
+
+int recv_msg(void)
+{
+ int ret;
+ int16_t c;
+
+ while (1) {
+ /* XXX use select for timeout */
+ c = uart_recv(GPS_UART);
+ ret = recv_cb(c);
+ if (ret == 0)
+ return 0;
+ }
+}
+
+int wait_ack(int msg_type)
+{
+ int ret;
+
+ while (1) {
+ ret = recv_msg();
+ if (ret < 0)
+ return -1;
+
+ /* retry if it's not the expected message */
+ if (rxframe.data[0] != 0x83 && rxframe.data[0] != 0x84)
+ continue;
+ if (rxframe.data[1] != msg_type)
+ continue;
+
+ if (rxframe.data[0] == 0x83)
+ printf("ACK\n");
+ else if (rxframe.data[0] == 0x84)
+ printf("NACK\n");
+ else
+ printf("ZARB\n");
+ break;
+ }
+
+ return 0;
+}
+
+static int decode_gps_pos(uint8_t *buf, uint16_t len)
+{
+ struct gps_pos *pos = (struct gps_pos *)buf;
+ uint8_t irq_flags;
+
+ if (len != sizeof(*pos))
+ return -1;
+
+ if (pos->msg_id != 0xA8) /* XXX not tested */
+ return -1;
+
+ gps_pos.gps_week = ntohs(pos->gps_week);
+ gps_pos.tow = ntohl(pos->tow);
+
+ gps_pos.latitude = ntohl(pos->latitude);
+ gps_pos.longitude = ntohl(pos->longitude);
+ gps_pos.altitude = ntohl(pos->altitude);
+
+ gps_pos.sea_altitude = ntohl(pos->sea_altitude);
+
+ gps_pos.gdop = ntohs(pos->gdop);
+ gps_pos.pdop = ntohs(pos->pdop);
+ gps_pos.hdop = ntohs(pos->hdop);
+ gps_pos.vdop = ntohs(pos->vdop);
+ gps_pos.tdop = ntohs(pos->tdop);
+
+ gps_pos.ecef_vx = ntohl(pos->ecef_vx);
+ gps_pos.ecef_vy = ntohl(pos->ecef_vy);
+ gps_pos.ecef_vz = ntohl(pos->ecef_vz);
+
+ /* update global structure */
+ IRQ_LOCK(irq_flags);
+ memcpy(&gps_pos, pos, sizeof(gps_pos));
+ IRQ_UNLOCK(irq_flags);
+
+ return 0;
+}
+
+/* display current GPS position stored in the global variable */
+static void display_gps(void)
+{
+ printf("id %.2X mode %.2X svnum %.2X gpsw %.4X tow %.10"PRIu32"\t",
+ gps_pos.msg_id,
+ gps_pos.mode,
+ gps_pos.sv_num,
+ gps_pos.gps_week,
+ gps_pos.tow);
+
+ printf("lat %.8"PRIx32" long %.8"PRIx32" alt %.8"PRIx32"\n",
+ gps_pos.latitude,
+ gps_pos.longitude,
+ gps_pos.altitude);
+
+ printf("gdop %3.3f pdop %3.3f hdop %3.3f vdop %3.3f tdop %3.3f\n",
+ (double)gps_pos.gdop/100.,
+ (double)gps_pos.pdop/100.,
+ (double)gps_pos.hdop/100.,
+ (double)gps_pos.vdop/100.,
+ (double)gps_pos.tdop/100.);
+
+ printf("lat %3.5f long %3.5f alt %3.5f sea_alt %3.5f\n",
+ (double)gps_pos.latitude/10000000.,
+ (double)gps_pos.longitude/10000000.,
+ (double)gps_pos.altitude/100.,
+ (double)gps_pos.sea_altitude/100.);
+
+ printf("vx %3.3f vy %3.3f vz %3.3f\n",
+ (double)gps_pos.ecef_vx/100.,
+ (double)gps_pos.ecef_vy/100.,
+ (double)gps_pos.ecef_vz/100.);
+}
+
+static void gps_venus_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+ int16_t c;
+ int ret;
+
+ (void)cm;
+ (void)tim;
+ (void)arg;
+
+ while (1) {
+ c = uart_recv_nowait(GPS_UART);
+ if (c < 0) /* no more char */
+ goto resched;
+
+ ret = recv_cb(c);
+ if (ret == 0) {
+ decode_gps_pos(rxframe.data, rxframe.len);
+ if (0)
+ display_gps();
+ }
+ }
+
+ resched:
+ callout_schedule(cm, tim, 2);
+}
+
+static void venus634_configure(void)
+{
+ /* ask the GPS to reset */
+ printf("init...");
+ venus634_restart();
+ wait_ack(M_RESTART);
+
+ /* it seems we must wait that the GPS is restarted, else it doesn't work
+ * properly */
+ wait_ms(500);
+
+ printf("binmode...");
+ venus634_msg_type();
+ wait_ack(M_OUTPUT);
+
+ printf("waas...");
+ venus634_waas();
+ wait_ack(M_WAAS);
+
+ printf("rate...");
+ venus634_rate();
+ wait_ack(M_RATE);
+
+ printf("GPS configuration done !\n");
+}
+
+/*
+ https://www.sparkfun.com/datasheets/GPS/Modules/AN0003_v1.4.14_FlashOnly.pdf
+*/
+
+int gps_venus_init(void)
+{
+ venus634_configure();
+
+ callout_init(&gps_timer, gps_venus_cb, NULL, GPS_PRIO);
+ callout_schedule(&imuboard.intr_cm, &gps_timer, 2); /* every 2ms */
+
+ return 0;
+}
+
+void gps_get_pos(struct gps_pos *pos)
+{
+ memcpy(pos, &gps_pos, sizeof(*pos));
+}
+
+int gps_loop(void)
+{
+ uint32_t ms;
+ uint8_t flags, prio;
+ int16_t len;
+ char buf[128];
+ struct gps_pos pos;
+
+ while (1) {
+
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ IRQ_UNLOCK(flags);
+
+ /* get position (prevent modification of gps pos during copy) */
+ prio = callout_mgr_set_prio(&imuboard.intr_cm, GPS_PRIO);
+ gps_get_pos(&pos);
+ callout_mgr_restore_prio(&imuboard.intr_cm, prio);
+
+ /* XXX copy */
+ len = snprintf(buf, sizeof(buf),
+ "%"PRIu32" "
+ "svnum %.2X lat %3.5f long %3.5f "
+ "alt %3.5f sea_alt %3.5f\n",
+ ms, gps_pos.sv_num,
+ (double)gps_pos.latitude/10000000.,
+ (double)gps_pos.longitude/10000000.,
+ (double)gps_pos.altitude/100.,
+ (double)gps_pos.sea_altitude/100.);
+
+
+ if (sd_log_enabled()) {
+
+ if (sd_log_write(buf, len) != len) {
+ printf_P(PSTR("error writing to file\n"));
+ return -1;
+ }
+ }
+ else
+ printf("%s", buf);
+
+ }
+
+ return 0;
+}
--- /dev/null
+#ifndef _GPS_VENUS_H
+#define _GPS_VENUS_H
+
+#include <stdint.h>
+
+/* A GPS position structure. It also contains some information about the number
+ * of seen satellites, the message ID, the date, ...
+ * See App Notes AN0028 p68 */
+struct gps_pos {
+ uint8_t msg_id; /* should be A8 */
+ uint8_t mode; /* Quality of fix 0: none, 1: 2D, 2: 3D, 3: 3D+DGNSS */
+ uint8_t sv_num; /* number of SV in fix (0-12) */
+
+ uint16_t gps_week; /* GNSS week number */
+ uint32_t tow; /* GNSS time of week */
+
+ int32_t latitude; /* between -90e7 and 90e7, in 1/1e-7 degrees,
+ * positive means north hemisphere */
+ int32_t longitude; /* between -180e7 and 180e7, in 1/1e-7 degrees,
+ * positive means east */
+ uint32_t altitude; /* altitude from elipsoid, in 1/100 meters */
+ uint32_t sea_altitude; /* altitude from sea level, in 1/100 meters */
+
+ uint16_t gdop; /* Geometric dilution of precision */
+ uint16_t pdop; /* Position dilution of precision */
+ uint16_t hdop; /* Horizontal dilution of precision */
+ uint16_t vdop; /* Vertical dilution of precision */
+ uint16_t tdop; /* Timec dilution of precision */
+
+ int32_t ecef_x; /* Earth-Centered, Earth-Fixed X pos, 1/100 meters */
+ int32_t ecef_y; /* Earth-Centered, Earth-Fixed Y pos, 1/100 meters */
+ int32_t ecef_z; /* Earth-Centered, Earth-Fixed Z pos, 1/100 meters */
+
+ int32_t ecef_vx; /* Earth-Centered, Earth-Fixed X speed, 1/100 m/s */
+ int32_t ecef_vy; /* Earth-Centered, Earth-Fixed Y speed, 1/100 m/s */
+ int32_t ecef_vz; /* Earth-Centered, Earth-Fixed Z speed, 1/100 m/s */
+
+} __attribute__ ((packed));
+
+int gps_venus_init(void);
+int gps_loop(void);
+
+/* does not lock intr, must be done by user */
+void gps_get_pos(struct gps_pos *pos);
+
+static inline int8_t gps_pos_valid(struct gps_pos *pos)
+{
+ /* XXX when a GPS position is valid ? */
+ return (pos->mode >= 2 && pos->sv_num >= 5);
+}
+
+#endif
--- /dev/null
+import sys, re
+import matplotlib
+matplotlib.use('QT4Agg')
+import numpy as np
+import matplotlib.pyplot as plt
+
+FLOAT = "([-+]?[0-9]*\.?[0-9]+)"
+INT = "([-+]?[0-9][0-9]*)"
+
+FILTER = 1
+
+tab_t = []
+tab_g1 = []
+tab_g2 = []
+tab_g3 = []
+tab_a1 = []
+tab_a2 = []
+tab_a3 = []
+tab_m1 = []
+tab_m2 = []
+tab_m3 = []
+
+f = open(sys.argv[1])
+while True:
+ l = f.readline()
+ if l == "":
+ break
+
+ regex = "%s.*gyro\s*%s\s*%s\s*%s\s*"%(INT, FLOAT, FLOAT, FLOAT)
+ regex += "accel\s*%s\s*%s\s*%s\s*"%(FLOAT, FLOAT, FLOAT)
+ regex += "magnet\s*%s\s*%s\s*%s\s*"%(FLOAT, FLOAT, FLOAT)
+
+ m = re.match(regex, l)
+ if m:
+ t, g1, g2, g3, a1, a2, a3, m1, m2, m3 = map(lambda x: float(x), m.groups())
+ tab_t.append(t)
+ tab_g1.append(g1)
+ tab_g2.append(g2)
+ tab_g3.append(g3)
+ tab_a1.append(a1)
+ tab_a2.append(a2)
+ tab_a3.append(a3)
+ tab_m1.append(m1)
+ tab_m2.append(m2)
+ tab_m3.append(m3)
+ print t, g1, g2, g3, a1, a2, a3, m1, m2, m3
+
+def mean(tab, n):
+ ret = []
+ for i in range(len(tab)-n):
+ s = reduce(lambda x,y:x+y, tab[i:i+n], 0)
+ ret.append(s/n)
+ return ret
+
+tab_a1 = mean(tab_a1, FILTER)
+tab_a2 = mean(tab_a2, FILTER)
+tab_a3 = mean(tab_a3, FILTER)
+tab_g1 = mean(tab_g1, FILTER)
+tab_g2 = mean(tab_g2, FILTER)
+tab_g3 = mean(tab_g3, FILTER)
+tab_m1 = mean(tab_m1, FILTER)
+tab_m2 = mean(tab_m2, FILTER)
+tab_m3 = mean(tab_m3, FILTER)
+
+# line, = plt.plot(tab_t[:-FILTER], tab_a1, 'r-')
+# line, = plt.plot(tab_t[:-FILTER], tab_a2, 'g-')
+# line, = plt.plot(tab_t[:-FILTER], tab_a3, 'b-')
+
+line, = plt.plot(tab_t[:-FILTER], tab_g1, 'r-')
+line, = plt.plot(tab_t[:-FILTER], tab_g2, 'g-')
+line, = plt.plot(tab_t[:-FILTER], tab_g3, 'b-')
+
+# line, = plt.plot(tab_t[:-FILTER], tab_m1, 'r-')
+# line, = plt.plot(tab_t[:-FILTER], tab_m2, 'g-')
+# line, = plt.plot(tab_t[:-FILTER], tab_m3, 'b-')
+
+
+#dashes = [10, 5, 100, 5] # 10 points on, 5 off, 100 on, 5 off
+#line.set_dashes(dashes)
+
+plt.show()
+
+
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_config.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $
+ *
+ */
+
+
+#define I2C_BITRATE 1 // divider dor i2c baudrate, see TWBR in doc
+#define I2C_PRESCALER 3 // prescaler config, rate = 2^(n*2)
+
+/* Size of transmission buffer */
+#define I2C_SEND_BUFFER_SIZE 32
+
+/* Size of reception buffer */
+#define I2C_RECV_BUFFER_SIZE 32
--- /dev/null
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+
+#include <i2c.h>
+
+#include "../common/i2c_commands.h"
+#include "gps_venus.h"
+#include "main.h"
+
+void i2c_protocol_init(void)
+{
+}
+
+/*** LED CONTROL ***/
+void i2c_led_control(uint8_t l, uint8_t state)
+{
+ switch(l) {
+ case 1:
+ state? LED1_ON():LED1_OFF();
+ break;
+ case 2:
+ state? LED2_ON():LED2_OFF();
+ break;
+ default:
+ break;
+ }
+}
+
+#ifdef notyet
+static void i2c_test(uint16_t val)
+{
+ static uint16_t prev=0;
+
+ if ( (val-prev) != 1 ) {
+ WARNING(E_USER_I2C_PROTO, "Duplicated msg %d", val);
+ }
+ prev = val;
+ ext.nb_test_cmd ++;
+}
+#endif
+
+static void i2c_send_status(void)
+{
+ struct i2c_ans_imuboard_status ans;
+ struct gps_pos gps_pos;
+ uint8_t irq_flags;
+
+ i2c_flush();
+ ans.hdr.cmd = I2C_ANS_IMUBOARD_STATUS;
+
+ /* gps */
+ IRQ_LOCK(irq_flags);
+ gps_get_pos(&gps_pos);
+ IRQ_UNLOCK(irq_flags);
+
+ ans.flags = 0;
+ if (gps_pos_valid(&gps_pos)) {
+ ans.flags |= IMUBOARD_STATUS_GPS_OK;
+ ans.latitude = gps_pos.latitude;
+ ans.longitude = gps_pos.longitude;
+ ans.altitude = gps_pos.altitude;
+ }
+
+ i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans,
+ sizeof(ans), I2C_CTRL_GENERIC);
+}
+
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+ void *void_cmd = buf;
+
+#if 0 /* XXX */
+ static uint8_t a = 0;
+
+ a++;
+ if (a & 0x10)
+ LED2_TOGGLE();
+
+ if (size <= 0) {
+ goto error;
+ }
+#endif
+
+ switch (buf[0]) {
+
+ /* Commands (no answer needed) */
+ case I2C_CMD_GENERIC_LED_CONTROL:
+ {
+ struct i2c_cmd_led_control *cmd = void_cmd;
+ if (size != sizeof (*cmd))
+ goto error;
+ i2c_led_control(cmd->led_num, cmd->state);
+ break;
+ }
+
+
+ /* Add other commands here ...*/
+
+ case I2C_REQ_IMUBOARD_STATUS:
+ {
+ //struct i2c_req_imuboard_status *cmd = void_cmd;
+ if (size != sizeof (struct i2c_req_imuboard_status))
+ goto error;
+
+ i2c_send_status();
+ break;
+ }
+
+ default:
+ goto error;
+ }
+
+ error:
+ /* log error on a led ? */
+ return;
+}
+
+void i2c_recvbyteevent(__attribute__((unused)) uint8_t hwstatus,
+ __attribute__((unused)) uint8_t i,
+ __attribute__((unused)) int8_t c)
+{
+}
+
+void i2c_sendevent(__attribute__((unused)) int8_t size)
+{
+}
+
+
--- /dev/null
+/*
+ * Copyright Droids Corporation (2007)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_protocol.h,v 1.1 2009-03-05 22:52:35 zer0 Exp $
+ *
+ */
+
+#include <aversive.h>
+
+void i2c_protocol_init(void);
+
+void i2c_recvevent(uint8_t * buf, int8_t size);
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
+void i2c_sendevent(int8_t size);
+
+int debug_send(char c, FILE* f);
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id$
+ *
+ */
+
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <aversive.h>
+
+#include <aversive/wait.h>
+
+
+#include <util/delay.h>
+
+
+#include "i2cm_sw.h"
+
+
+
+
+volatile i2cm_state g_i2cm_state = NOT_INIT;
+volatile uint8_t g_i2cm_byte;
+
+void (*g_i2cm_event)(i2cm_state state);
+
+/*
+ Mini Arduino
+ I2C:
+ A4 = PC4/SDA
+ A5 = PC5/SCL
+*/
+
+#define I2CM_SCL_PORT PORTD
+#define I2CM_SCL_BIT 6
+
+#define I2CM_SDA_PORT PORTD
+#define I2CM_SDA_BIT 5
+
+#define I2CM_SCL I2CM_SCL_PORT, I2CM_SCL_BIT
+#define I2CM_SDA I2CM_SDA_PORT, I2CM_SDA_BIT
+
+
+
+
+void i2cm_init(void)
+{
+ /*
+ - port SCL/SDA is set to 0
+ - i2c is then driven using DDR 0 (Hi-Z) or 1 (For Low)
+ */
+
+ // SCL high
+ cbi(DDR(I2CM_SCL_PORT),I2CM_SCL_BIT);
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+
+ // SDA high
+ cbi(DDR(I2CM_SDA_PORT),I2CM_SDA_BIT);
+ I2C_HIGH(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_DELAY();
+
+ g_i2cm_state = I2CM_READY;
+}
+
+
+void i2cm_manage(void)
+{
+ if ( (g_i2cm_event != NULL) && (g_i2cm_state != I2CM_READY) ) {
+ g_i2cm_event( g_i2cm_state );
+ // reset notification
+ if (g_i2cm_state != I2CM_BUSY)
+ g_i2cm_state = I2CM_READY;
+ }
+}
+
+uint8_t i2cm_get_state(void)
+{
+ return g_i2cm_state;
+}
+
+
+uint8_t i2cm_get_received_byte(void)
+{
+ g_i2cm_state = I2CM_READY;
+ return g_i2cm_byte;
+}
+
+void i2cm_register_event(void (*func)(i2cm_state state))
+{
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ g_i2cm_event = func;
+ IRQ_UNLOCK(flags);
+}
+
+
+static uint8_t i2cm_send_byte(uint8_t byte)
+{
+ uint8_t mask;
+ uint8_t err = 0;
+
+
+ g_i2cm_state = I2CM_BUSY;
+
+ // SCL should already be low
+ // I2C_LOW(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ // I2CM_DELAY();
+
+ mask = 0x80;
+ while (mask) {
+
+
+ // data out
+ if (mask & byte)
+ I2C_HIGH(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ else
+ I2C_LOW(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_DELAY();
+ mask >>=1;
+
+ // clock High
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+ while ( bit_is_clear(PIN(I2CM_SCL_PORT), I2CM_SCL_BIT) );// slave handshake
+
+ // clock low
+ I2C_LOW(I2CM_SCL_PORT, I2CM_SCL_BIT);
+
+ }
+
+ /* receive ack */
+
+ // release SDA
+ I2C_HIGH(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_BIT_DELAY();
+
+ // clock HIGH
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+ while ( bit_is_clear(PIN(I2CM_SCL_PORT), I2CM_SCL_BIT) );// slave handshake
+
+ // we should receive ACK
+ if (bit_is_set(PIN(I2CM_SDA_PORT), I2CM_SDA_BIT))
+ err = I2CM_SENT_NO_ACK;
+
+ // clock low
+ I2C_LOW(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+
+ return err;
+}
+
+
+uint8_t i2cm_send_start(uint8_t sla_w)
+{
+ uint8_t err;
+ uint16_t i;
+ g_i2cm_state = I2CM_BUSY;
+
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2C_HIGH(I2CM_SDA_PORT, I2CM_SDA_BIT);
+
+ I2CM_DELAY();
+
+ /* wait for bus realese */
+ for (i=0;i<0x8000; i++){
+ if( bit_is_set(PIN(I2CM_SCL_PORT), I2CM_SCL_BIT) && \
+ bit_is_set(PIN(I2CM_SDA_PORT), I2CM_SDA_BIT) )
+ break;
+ }
+
+ // while ( bit_is_clear(PIN(I2CM_SCL_PORT), I2CM_SCL_BIT) );// slave handshake
+
+ // start condition
+ I2C_LOW(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_DELAY();
+ I2C_LOW(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+
+ err = i2cm_send_byte(sla_w);
+ return err;
+}
+
+
+
+uint8_t i2cm_send_stop(void)
+{
+ g_i2cm_state = I2CM_BUSY;
+
+ // data down
+ I2C_LOW(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2C_LOW(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_DELAY();
+
+ // stop condition
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+ I2C_HIGH(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_DELAY();
+
+ g_i2cm_state = I2CM_SENT_STOP;
+ return 0;
+}
+
+
+uint8_t i2cm_receive_byte(uint8_t last)
+{
+ uint8_t mask,data;
+
+ g_i2cm_state = I2CM_BUSY;
+
+ I2C_HIGH(I2CM_SDA_PORT, I2CM_SDA_BIT);
+
+
+ data = 0;
+ mask = 0x80;
+ while (mask) {
+
+ // clock High
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+ while ( bit_is_clear(PIN(I2CM_SCL_PORT), I2CM_SCL_BIT) );// slave handshake
+
+ if (bit_is_set(PIN(I2CM_SDA_PORT),I2CM_SDA_BIT))
+ data |= mask;
+
+ // clock low
+ I2C_LOW(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+
+ mask >>=1;
+ }
+
+ if (!last){
+ /* send ack */
+ I2C_LOW(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_BIT_DELAY();
+ }
+
+ // clock HIGH
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+ while ( bit_is_clear(PIN(I2CM_SCL_PORT), I2CM_SCL_BIT) );// slave handshake
+
+ // clock low
+ I2C_LOW(I2CM_SCL_PORT, I2CM_SCL_BIT);
+ I2CM_DELAY();
+
+ if (!last) {
+ // release DATA
+ I2C_HIGH(I2CM_SDA_PORT, I2CM_SDA_BIT);
+ I2CM_BIT_DELAY();
+ }
+
+ g_i2cm_byte = data;
+ g_i2cm_state = I2CM_RECEIVED_BYTE;
+
+ I2C_HIGH(I2CM_SCL_PORT, I2CM_SCL_BIT);
+
+ return 0;
+}
+
+
+uint8_t i2cm_send(uint8_t addr, uint8_t* data, uint8_t len)
+{
+ uint8_t i;
+ uint8_t err = 0;
+
+ err = i2cm_send_start((addr<<1) | 0);
+ if (err)
+ return err;
+
+ for (i=0; i<len; i++) {
+ err = i2cm_send_byte(data[i]);
+ if (err)
+ break;
+ }
+
+ i2cm_send_stop();
+ return err;
+}
+
+
+uint8_t i2c_buf[0x20]; /* XXX */
+
+uint8_t i2cm_recv(uint8_t addr, uint8_t len)
+{
+ uint8_t i;
+ uint8_t err = 0;
+
+ err = i2cm_send_start((addr<<1) | 1);
+ if (err)
+ return err;
+
+ I2CM_DELAY();
+
+ for (i=0; i<len; i++){
+ err =i2cm_receive_byte(i == len-1);
+ i2c_buf[i] = g_i2cm_byte;
+ if (err)
+ break;
+ }
+ i2cm_send_stop();
+ return err;
+}
+
+
+uint8_t i2cm_get_recv_buffer(uint8_t* buf, uint8_t len)
+{
+ uint8_t i;
+ for (i=0; i<len; i++)
+ buf[i] = i2c_buf[i];
+ return len;
+}
--- /dev/null
+#ifndef I2CM_SW_H
+#define I2CM_SW_H
+
+/*
+#define BIT(x) (1 << (x))
+#define SETBITS(x,y) ((x) |= (y))
+#define CLEARBITS(x,y) ((x) &= (~(y)))
+#define SETBIT(x,y) SETBITS((x), (BIT((y))))
+#define CLEARBIT(x,y) CLEARBITS((x), (BIT((y))))
+#define BITSET(x,y) ((x) & (BIT(y)))
+#define BITCLEAR(x,y) !BITSET((x), (y))
+#define BITSSET(x,y) (((x) & (y)) == (y))
+#define BITSCLEAR(x,y) (((x) & (y)) == 0)
+#define BITVAL(x,y) (((x)>>(y)) & 1)
+*/
+
+#define I2C_LOW(port, bit) sbi(DDR(port),bit)
+#define I2C_HIGH(port, bit) cbi(DDR(port),bit)
+
+
+typedef uint8_t i2cm_state;
+
+#define NOT_INIT 0
+#define I2CM_READY 1
+#define I2CM_BUSY 2
+#define I2CM_SENT 3
+#define I2CM_SENT_NO_ACK 4
+#define I2CM_SENT_START 5
+#define I2CM_SENT_START_NO_ACK 6
+#define I2CM_SENT_STOP 7
+#define I2CM_RECEIVED_BYTE 8
+
+#define I2CM_DELAY() _delay_loop_2(4)
+#define I2CM_BIT_DELAY() _delay_loop_2(4)
+
+void i2cm_init(void);
+void i2cm_manage(void);
+uint8_t i2cm_get_state(void);
+uint8_t i2cm_get_received_byte(void);
+void i2cm_register_event(void (*func)(i2cm_state state));
+uint8_t i2cm_send_start(uint8_t sla_w);
+uint8_t i2cm_send_stop(void);
+uint8_t i2cm_receive_byte(uint8_t last);
+
+uint8_t i2cm_send(uint8_t addr, uint8_t* data, uint8_t len);
+uint8_t i2cm_recv(uint8_t addr, uint8_t len);
+uint8_t i2cm_get_recv_buffer(uint8_t* buf, uint8_t len);
+
+
+#define I2C_ERR_SEND_START 1
+#define I2C_ERR_SEND_BYTE 2
+#define I2C_ERR_RECV_BYTE 3
+
+
+#endif //I2CM_SW_H
--- /dev/null
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#include <aversive/irq_lock.h>
+#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
+
+#include <uart.h>
+
+#include "mpu6050.h"
+#include "matrix.h"
+#include "MadgwickAHRS.h"
+#include "sd_log.h"
+#include "imu.h"
+#include "main.h"
+
+/* structure storing the latest imu infos returned by the sensor */
+static struct imu_info g_imu;
+
+/* structure storing the latest quaternion */
+static struct quaternion g_quat;
+
+/* structure storing the latest euler position */
+static struct euler g_euler;
+
+/* periodical timer structure */
+static struct callout imu_timer;
+
+static void quaternion2euler(const struct quaternion *quat, struct euler *euler)
+{
+ float q0 = quat->q0;
+ float q1 = quat->q1;
+ float q2 = quat->q2;
+ float q3 = quat->q3;
+
+ euler->roll = atan2f(2.0f * (q0 * q1 + q2 * q3),
+ q0*q0 - q1*q1 - q2*q2 + q3*q3);
+ euler->pitch = -asinf(2.0f * (q1 * q3 - q0 * q2));
+ euler->yaw = atan2f(2.0f * (q1 * q2 + q0 * q3),
+ q0*q0 + q1*q1 - q2*q2 - q3*q3);
+}
+
+/* timer callback that polls IMU and does the calculation */
+static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+ struct imu_info imu;
+ struct quaternion quat;
+ struct euler euler;
+ uint8_t irq_flags;
+
+ (void)arg;
+
+ /* copy previous quaternion locally */
+ IRQ_LOCK(irq_flags);
+ memcpy(&quat, &g_quat, sizeof(quat));
+ IRQ_UNLOCK(irq_flags);
+
+ mpu6050_read_all_axes(&g_imu);
+ MadgwickAHRSupdate(&g_imu, &g_quat);
+ quaternion2euler(&g_quat, &g_euler);
+
+ /* update global variables */
+ IRQ_LOCK(irq_flags);
+ memcpy(&g_imu, &imu, sizeof(g_imu));
+ IRQ_UNLOCK(irq_flags);
+ IRQ_LOCK(irq_flags);
+ memcpy(&g_quat, &quat, sizeof(g_quat));
+ IRQ_UNLOCK(irq_flags);
+ IRQ_LOCK(irq_flags);
+ memcpy(&g_euler, &euler, sizeof(g_euler));
+ IRQ_UNLOCK(irq_flags);
+
+ /* reschedule event */
+ callout_schedule(cm, tim, 2);
+}
+
+void imu_init(void)
+{
+ mpu6050_init();
+
+ callout_init(&imu_timer, imu_cb, NULL, IMU_PRIO);
+ callout_schedule(&imuboard.intr_cm, &imu_timer, 20); /* every 20ms */
+}
+
+void imu_get_info(struct imu_info *imu)
+{
+ memcpy(imu, &g_imu, sizeof(*imu));
+}
+
+void imu_get_pos_quat(struct quaternion *quat)
+{
+ memcpy(quat, &g_quat, sizeof(*quat));
+}
+
+void imu_get_pos_euler(struct euler *euler)
+{
+ memcpy(euler, &g_euler, sizeof(*euler));
+}
+
+
+int imu_log(void)
+{
+ char buf[128];
+ int16_t len;
+ uint32_t ms;
+ uint8_t flags;
+ struct imu_info imu;
+
+ if (sd_log_enabled() == 0)
+ return 0;
+
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ imu_get_info(&imu);
+ IRQ_UNLOCK(flags);
+
+ len = snprintf(buf, sizeof(buf),
+ "%"PRIu32"\t"
+ "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t"
+ "accel %+3.3f\t%+3.3f\t%+3.3f\t\t"
+ "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n",
+ ms,
+ imu.gx, imu.gy, imu.gz,
+ imu.ax, imu.ay, imu.az,
+ imu.mx, imu.my, imu.mz);
+ if (sd_log_write(buf, len) != len) {
+ printf_P(PSTR("error writing to file\n"));
+ return -1;
+ }
+
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef IMU_H_
+#define IMU_H_
+
+struct imu_info {
+ /* gyro */
+ double gx;
+ double gy;
+ double gz;
+
+ /* accelero */
+ double ax;
+ double ay;
+ double az;
+
+ /* magneto */
+ double mx;
+ double my;
+ double mz;
+
+ /* temperature */
+ double temp;
+};
+
+struct quaternion {
+ double q0;
+ double q1;
+ double q2;
+ double q3;
+};
+
+struct euler {
+ double roll;
+ double pitch;
+ double yaw;
+};
+
+/* initialize the IMU */
+void imu_init(void);
+
+/* if sd log file is opened, log the status of IMU on the sdcard */
+int imu_log(void);
+
+/* return a filled imu_info structure corresponding to the latest axes read in
+ * the timer callback. Does not lock irq, so it's up to the user to do that. */
+void imu_get_info(struct imu_info *imu);
+
+/* return the latest position in a quaternion struct read in the timer
+ * callback. Does not lock irq, so it's up to the user to do that. */
+void imu_get_pos_quat(struct quaternion *pos);
+
+/* return the latest position in an euler struct read in the timer
+ * callback. Does not lock irq, so it's up to the user to do that. */
+void imu_get_pos_euler(struct euler *pos);
+
+#endif
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* fuses:
+ * avrdude -p atmega1284p -P usb -c avrispmkii -U lfuse:w:0xff:m -U hfuse:w:0x91:m -U efuse:w:0xff:m
+ * -> it failed but I answered y, then make reset and it was ok
+ */
+
+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/endian.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <inttypes.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <errno.h>
+#include <ctype.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <timer.h>
+#include <i2cm_sw.h>
+#include <i2c.h>
+
+#include "eeprom_config.h"
+#include "gps_venus.h"
+#include "sd_log.h"
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
+#include "imu.h"
+#include "main.h"
+
+struct imuboard imuboard;
+volatile uint32_t global_ms;
+
+/* global xbee device */
+struct xbee_dev *xbee_dev;
+
+void bootloader(void)
+{
+#define BOOTLOADER_ADDR 0x3f000
+ if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
+ printf_P(PSTR("Bootloader is not present\r\n"));
+ return;
+ }
+ cli();
+ /* ... very specific :( */
+ TIMSK0 = 0;
+ TIMSK1 = 0;
+ TIMSK2 = 0;
+ TIMSK3 = 0;
+ EIMSK = 0;
+ UCSR0B = 0;
+ UCSR1B = 0;
+ SPCR = 0;
+ TWCR = 0;
+ ACSR = 0;
+ ADCSRA = 0;
+
+ /* XXX */
+ /* __asm__ __volatile__ ("ldi r31,0xf8\n"); */
+ /* __asm__ __volatile__ ("ldi r30,0x00\n"); */
+ /* __asm__ __volatile__ ("eijmp\n"); */
+}
+
+/* return time in milliseconds on unsigned 16 bits */
+uint16_t get_time_ms(void)
+{
+ uint16_t ms;
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ IRQ_UNLOCK(flags);
+ return ms;
+}
+
+static void main_timer_interrupt(void)
+{
+ static uint16_t cycles;
+ static uint8_t stack = 0;
+ static uint8_t cpt = 0;
+ cpt++;
+
+ /* LED blink */
+ if (global_ms & 0x80)
+ LED1_ON();
+ else
+ LED1_OFF();
+
+ if ((cpt & 0x03) != 0)
+ return;
+
+ /* the following code is only called one interrupt among 4: every 682us
+ * (at 12 Mhz) = 8192 cycles */
+ cycles += 8192;
+ if (cycles >= 12000) {
+ cycles -= 12000;
+ global_ms ++;
+ }
+
+ /* called */
+ if (stack++ == 0)
+ LED2_ON();
+ sei();
+ if ((cpt & 0x3) == 0)
+ callout_manage(&imuboard.intr_cm);
+ cli();
+ if (--stack == 0)
+ LED2_OFF();
+}
+
+/* XXX */
+int sd_main(void);
+
+int main(void)
+{
+ DDRB = 0x18 /* LEDs */;
+
+ uart_init();
+ uart_register_rx_event(CMDLINE_UART, emergency);
+
+ fdevopen(cmdline_dev_send, cmdline_dev_recv);
+ timer_init();
+ timer0_register_OV_intr(main_timer_interrupt);
+
+ callout_mgr_init(&imuboard.intr_cm, get_time_ms);
+
+ cmdline_init();
+ /* LOGS */
+ error_register_emerg(mylog);
+ error_register_error(mylog);
+ error_register_warning(mylog);
+ error_register_notice(mylog);
+ error_register_debug(mylog);
+
+ /* communication with mpu6050 */
+ i2cm_init();
+
+ /* i2c hw to communicate with mainboard */
+ i2c_init(I2C_MODE_SLAVE, I2C_IMUBOARD_ADDR);
+ i2c_protocol_init();
+ i2c_register_recv_event(i2c_recvevent);
+ i2c_register_send_event(i2c_sendevent);
+
+ eeprom_load_config();
+
+ sd_log_open();
+
+ sei();
+
+ printf_P(PSTR("\r\n"));
+ rdline_newline(&imuboard.rdl, imuboard.prompt);
+
+ //sd_main();
+ imu_init();
+ //imu_loop();
+ gps_venus_init();
+ gps_loop();
+
+ while (1) {
+ cmdline_poll();
+ }
+
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _MAIN_H_
+#define _MAIN_H_
+
+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/pgmspace.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <parse.h>
+#include <rdline.h>
+#include <timer.h>
+#include <xbee.h>
+#include <callout.h>
+
+#include "cmdline.h"
+
+#define NB_LOGS 4
+
+/** ERROR NUMS */
+#define E_USER_DEFAULT 194
+#define E_USER_XBEE 195
+#define E_USER_RC_PROTO 196
+
+#define LED1_ON() sbi(PORTB, 3)
+#define LED1_OFF() cbi(PORTB, 3)
+
+#define LED2_ON() sbi(PORTB, 4)
+#define LED2_OFF() cbi(PORTB, 4)
+
+/* highest priority */
+#define LED_PRIO 160
+#define TIME_PRIO 140
+#define GPS_PRIO 80
+#define IMU_PRIO 70
+#define LOW_PRIO 60
+/* lowest priority */
+
+#define MAX_POWER_LEVEL 5
+/* generic to all boards */
+struct imuboard {
+ /* command line interface */
+ struct rdline rdl;
+ char prompt[RDLINE_PROMPT_SIZE];
+
+ struct callout_mgr intr_cm;
+
+ /* log */
+ uint8_t logs[NB_LOGS+1];
+ uint8_t log_level;
+ uint8_t debug;
+};
+extern struct imuboard imuboard;
+
+extern volatile uint32_t global_ms;
+
+void bootloader(void);
+uint16_t get_time_ms(void);
+
+#endif /* _MAIN_H_ */
--- /dev/null
+//Multiply two 3x3 matrixs. This function developed by Jordi can be easily adapted to multiple n*n matrix's. (Pero me da flojera!).
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3])
+{
+ float op[3];
+ for(int x=0; x<3; x++) {
+ for(int y=0; y<3; y++) {
+ for(int w=0; w<3; w++) {
+ op[w]=a[x][w]*b[w][y];
+ }
+ mat[x][y]=0;
+ mat[x][y]=op[0]+op[1]+op[2];
+
+ float test=mat[x][y];
+ }
+ }
+}
--- /dev/null
+#ifndef _MATRIX_H_
+#define _MATRIX_H_
+
+// Multiply two 3x3 matrixs.
+void Matrix_Multiply(float a[3][3], float b[3][3],float mat[3][3]);
+#endif// _MATRIX_H_
--- /dev/null
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/wait.h>
+
+#include "i2cm_sw.h"
+#include "mpu6050.h"
+#include "mpu6050_regs.h"
+
+#define ToRad(x) ((x) * 0.01745329252) // *pi/180
+
+// fs_sel = 1
+// 500°/s
+/* #define gyro_x_resolution 66.5 */
+/* #define gyro_y_resolution 66.5 */
+/* #define gyro_z_resolution 66.5 */
+
+// fs_sel = 3
+// 2000°/s
+#define gyro_x_resolution 16.4
+#define gyro_y_resolution 16.4
+#define gyro_z_resolution 16.4
+
+#define accel_x_resolution 2048.0
+#define accel_y_resolution 2048.0
+#define accel_z_resolution 2048.0
+
+#define MPU6050_ADDRESS 0x69
+#define MPU6050_MAGNETO_ADDRESS 0x0D
+
+#define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
+
+int16_t drift_g[3] = {0, 0, 0};
+
+/* read "len" bytes of mpu6050 registers starting at specified address */
+static uint8_t read_reg_len(uint8_t i2c_addr, uint8_t reg_addr,
+ uint8_t *values, uint8_t len)
+{
+ uint8_t err = 0;
+
+ err = i2cm_send(i2c_addr, ®_addr, 1);
+ if (err) {
+ printf("read reg len: i2c error send\r\n");
+ return err;
+ }
+
+ err = i2cm_recv(i2c_addr, len);
+ if (err) {
+ printf("read reg len: i2c error recv\r\n");
+ return err;
+ }
+
+ err = i2cm_get_recv_buffer(values, len);
+ if (err != len) {
+ printf("read reg len: i2c error get recv\r\n");
+ return 0xFF;
+ }
+
+ return 0;
+}
+
+/* read one byte of mpu6050 register at specified address */
+static uint8_t read_reg(uint8_t i2c_addr, uint8_t reg_addr,
+ uint8_t *value)
+{
+ return read_reg_len(i2c_addr, reg_addr, value, 1);;
+}
+
+/* fill the axes[3] pointer with the 3 axes of gyro (16bits) */
+uint8_t mpu6050_read_gyro_raw(int16_t *axes)
+{
+ uint8_t err;
+ uint8_t i;
+
+ err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H,
+ (uint8_t *)axes, sizeof(int16_t) * 3);
+ for (i = 0; i < 3; i++) {
+ axes[i] = SWAP_16(axes[i]);
+ }
+
+ return err;
+}
+
+/* fill the imu structure with axes comming from mpu6050 */
+uint8_t mpu6050_read_all_axes(struct imu_info *imu)
+{
+ int16_t axes[10];
+ uint8_t err;
+ uint8_t i;
+
+ err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H,
+ (uint8_t *)axes, sizeof(axes));
+
+ for (i = 0; i < 7; i++) {
+ axes[i] = SWAP_16(axes[i]);
+ }
+
+ imu->ax = 9.81 * (double)axes[0] / accel_x_resolution ;
+ imu->ay = 9.81 * (double)axes[1] / accel_y_resolution ;
+ imu->az = 9.81 * (double)axes[2] / accel_z_resolution ;
+
+ imu->temp = (double)axes[3]/340. + 36.5;
+
+ imu->gx = ToRad((double)(axes[4] - drift_g[0]) / gyro_x_resolution);
+ imu->gy = ToRad((double)(axes[5] - drift_g[1]) / gyro_y_resolution);
+ imu->gz = ToRad((double)(axes[6] - drift_g[2]) / gyro_z_resolution);
+
+ imu->mx = (double) axes[7] * 0.3;
+ imu->my = (double) axes[8] * 0.3;
+ imu->mz = (double) axes[9] * 0.3;
+
+ return err;
+}
+
+
+/* write a 8bits value at the specified register of the mpu6050 */
+static uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
+{
+ uint8_t err;
+ uint8_t buffer[2];
+ uint8_t check = 0;
+
+ buffer[0] = reg;
+ buffer[1] = val;
+
+ err = i2cm_send(address, (unsigned char*)buffer, 2);
+ if (err)
+ printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
+
+ err = read_reg(address, reg, &check);
+ if (err)
+ return err;
+
+ if (check != val) {
+ printf("reg %x: %x != %x\r\n", reg, check, val);
+ return 0xff;
+ }
+
+ return err;
+}
+
+/* XXX add comment */
+static void mpu6050_compute_drift(void)
+{
+ uint16_t i;
+ int32_t s_gx, s_gy, s_gz;
+ int16_t g_values[3];
+
+ drift_g[0] = 0;
+ drift_g[1] = 0;
+ drift_g[2] = 0;
+
+ s_gx = s_gy = s_gz = 0;
+
+ for (i = 0; i < 0x100; i ++) {
+ mpu6050_read_gyro_raw(g_values);
+ s_gx += g_values[0];
+ s_gy += g_values[1];
+ s_gz += g_values[2];
+ }
+ printf("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n", s_gx, s_gy, s_gz, i);
+ s_gx /= i;
+ s_gy /= i;
+ s_gz /= i;
+
+ drift_g[0] = s_gx;
+ drift_g[1] = s_gy;
+ drift_g[2] = s_gz;
+ printf("gyro drift:\r\n");
+ printf("%d %d %d\r\n",
+ drift_g[0],
+ drift_g[1],
+ drift_g[2]);
+}
+
+static uint8_t setup_mpu9150_magneto(void)
+{
+ //uint8_t i, err;
+
+ //MPU9150_I2C_ADDRESS = 0x0C; //change Adress to Compass
+
+ /* XXX doesn't work, no answer from magneto */
+ /* for (i = 0; i < 127; i++) { */
+ /* printf("i=%d\r\n", i); */
+ /* err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
+
+ /* if (err == 0) */
+ /* printf("COIN\r\n"); */
+ /* } */
+ send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
+ send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
+ send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
+
+ //MPU9150_I2C_ADDRESS = 0x69; //change Adress to MPU
+
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
+ //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
+ //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
+
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
+ send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
+
+ return 0;
+}
+
+int8_t mpu6050_init(void)
+{
+ uint8_t err;
+ uint8_t id = 0;
+
+ wait_ms(1000); /* XXX needed ? */
+
+ while (1) { /* XXX timeout */
+ err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
+ if (err)
+ continue;
+ if (id == 0x68)
+ break;
+ }
+
+ /* XX use one of the gyro for clock ref: if we don't do that, some i2c
+ * commands fail... why ?? */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
+
+ /* Sets sample rate to 1000/1+1 = 500Hz */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
+ /* Disable FSync, 48Hz DLPF */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
+ /* Disable gyro self tests, scale of 500 degrees/s */
+ /* send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); */
+ /* Disable gyro self tests, scale of 2000 degrees/s */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
+
+ /* Disable accel self tests, scale of +-16g, no DHPF */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
+
+ /* Freefall threshold of <|0mg| */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
+ /* Freefall duration limit of 0 */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
+ /* Motion threshold of >0mg */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
+ /* Motion duration of >0s */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
+ /* Zero motion threshold */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
+ /* Zero motion duration threshold */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
+ /* Disable sensor output to FIFO buffer */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
+
+ /* AUX I2C setup */
+ /* Sets AUX I2C to single master control, plus other config */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
+ /* Setup AUX I2C slaves */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
+
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
+
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
+
+ /* Setup INT pin and AUX I2C pass through */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
+ /* Enable data ready interrupt */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
+
+ /* Slave out, dont care */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
+
+ /* More slave config */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
+ /* Reset sensor signal paths */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
+ /* Motion detection control */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
+ /* Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
+ /* Sets clock source to gyro reference w/ PLL */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
+ /* Controls frequency of wakeups in accel low power mode plus the sensor
+ * standby modes */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
+ /* Data transfer to and from the FIFO buffer */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
+
+ setup_mpu9150_magneto();
+
+ printf("MPU6050 Setup Complete\r\n");
+ mpu6050_compute_drift();
+ printf("MPU6050 drift computed\r\n");
+
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef MPU6050_H_
+#define MPU6050_H_
+
+#include "imu.h"
+
+/* initialize the sensor, and calibrate it */
+int8_t mpu6050_init(void);
+
+/* fill the axes[3] pointer with the 3 axes of gyro (16bits) */
+uint8_t mpu6050_read_gyro_raw(int16_t *values);
+
+/* fill the imu structure with axes comming from mpu6050 */
+uint8_t mpu6050_read_all_axes(struct imu_info *imu);
+
+#endif /* MPU6050_H_ */
--- /dev/null
+
+#ifndef _MPU6050_REGS_H_
+#define _MPU6050_REGS_H_
+
+#define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
+#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
+#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
+#define MPU6050_RA_XA_OFFS_H 0x06 //[15:0] XA_OFFS
+#define MPU6050_RA_XA_OFFS_L_TC 0x07
+#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
+#define MPU6050_RA_YA_OFFS_L_TC 0x09
+#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
+#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
+#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
+#define MPU6050_RA_XG_OFFS_USRL 0x14
+#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
+#define MPU6050_RA_YG_OFFS_USRL 0x16
+#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
+#define MPU6050_RA_ZG_OFFS_USRL 0x18
+#define MPU6050_RA_SMPLRT_DIV 0x19
+#define MPU6050_RA_CONFIG 0x1A
+#define MPU6050_RA_GYRO_CONFIG 0x1B
+#define MPU6050_RA_ACCEL_CONFIG 0x1C
+#define MPU6050_RA_FF_THR 0x1D
+#define MPU6050_RA_FF_DUR 0x1E
+#define MPU6050_RA_MOT_THR 0x1F
+#define MPU6050_RA_MOT_DUR 0x20
+#define MPU6050_RA_ZRMOT_THR 0x21
+#define MPU6050_RA_ZRMOT_DUR 0x22
+#define MPU6050_RA_FIFO_EN 0x23
+#define MPU6050_RA_I2C_MST_CTRL 0x24
+#define MPU6050_RA_I2C_SLV0_ADDR 0x25
+#define MPU6050_RA_I2C_SLV0_REG 0x26
+#define MPU6050_RA_I2C_SLV0_CTRL 0x27
+#define MPU6050_RA_I2C_SLV1_ADDR 0x28
+#define MPU6050_RA_I2C_SLV1_REG 0x29
+#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
+#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
+#define MPU6050_RA_I2C_SLV2_REG 0x2C
+#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
+#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
+#define MPU6050_RA_I2C_SLV3_REG 0x2F
+#define MPU6050_RA_I2C_SLV3_CTRL 0x30
+#define MPU6050_RA_I2C_SLV4_ADDR 0x31
+#define MPU6050_RA_I2C_SLV4_REG 0x32
+#define MPU6050_RA_I2C_SLV4_DO 0x33
+#define MPU6050_RA_I2C_SLV4_CTRL 0x34
+#define MPU6050_RA_I2C_SLV4_DI 0x35
+#define MPU6050_RA_I2C_MST_STATUS 0x36
+#define MPU6050_RA_INT_PIN_CFG 0x37
+#define MPU6050_RA_INT_ENABLE 0x38
+#define MPU6050_RA_DMP_INT_STATUS 0x39
+#define MPU6050_RA_INT_STATUS 0x3A
+#define MPU6050_RA_ACCEL_XOUT_H 0x3B
+#define MPU6050_RA_ACCEL_XOUT_L 0x3C
+#define MPU6050_RA_ACCEL_YOUT_H 0x3D
+#define MPU6050_RA_ACCEL_YOUT_L 0x3E
+#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
+#define MPU6050_RA_ACCEL_ZOUT_L 0x40
+#define MPU6050_RA_TEMP_OUT_H 0x41
+#define MPU6050_RA_TEMP_OUT_L 0x42
+#define MPU6050_RA_GYRO_XOUT_H 0x43
+#define MPU6050_RA_GYRO_XOUT_L 0x44
+#define MPU6050_RA_GYRO_YOUT_H 0x45
+#define MPU6050_RA_GYRO_YOUT_L 0x46
+#define MPU6050_RA_GYRO_ZOUT_H 0x47
+#define MPU6050_RA_GYRO_ZOUT_L 0x48
+#define MPU6050_RA_EXT_SENS_DATA_00 0x49
+#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
+#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
+#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
+#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
+#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
+#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
+#define MPU6050_RA_EXT_SENS_DATA_07 0x50
+#define MPU6050_RA_EXT_SENS_DATA_08 0x51
+#define MPU6050_RA_EXT_SENS_DATA_09 0x52
+#define MPU6050_RA_EXT_SENS_DATA_10 0x53
+#define MPU6050_RA_EXT_SENS_DATA_11 0x54
+#define MPU6050_RA_EXT_SENS_DATA_12 0x55
+#define MPU6050_RA_EXT_SENS_DATA_13 0x56
+#define MPU6050_RA_EXT_SENS_DATA_14 0x57
+#define MPU6050_RA_EXT_SENS_DATA_15 0x58
+#define MPU6050_RA_EXT_SENS_DATA_16 0x59
+#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
+#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
+#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
+#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
+#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
+#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
+#define MPU6050_RA_EXT_SENS_DATA_23 0x60
+#define MPU6050_RA_MOT_DETECT_STATUS 0x61
+#define MPU6050_RA_I2C_SLV0_DO 0x63
+#define MPU6050_RA_I2C_SLV1_DO 0x64
+#define MPU6050_RA_I2C_SLV2_DO 0x65
+#define MPU6050_RA_I2C_SLV3_DO 0x66
+#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
+#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
+#define MPU6050_RA_MOT_DETECT_CTRL 0x69
+#define MPU6050_RA_USER_CTRL 0x6A
+#define MPU6050_RA_PWR_MGMT_1 0x6B
+#define MPU6050_RA_PWR_MGMT_2 0x6C
+#define MPU6050_RA_BANK_SEL 0x6D
+#define MPU6050_RA_MEM_START_ADDR 0x6E
+#define MPU6050_RA_MEM_R_W 0x6F
+#define MPU6050_RA_DMP_CFG_1 0x70
+#define MPU6050_RA_DMP_CFG_2 0x71
+#define MPU6050_RA_FIFO_COUNTH 0x72
+#define MPU6050_RA_FIFO_COUNTL 0x73
+#define MPU6050_RA_FIFO_R_W 0x74
+#define MPU6050_RA_WHO_AM_I 0x75
+
+#endif // _MPU6050_REGS_H_
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#include "byteordering.h"
+#include "partition.h"
+#include "partition_config.h"
+#include "sd-reader_config.h"
+
+#include <string.h>
+
+#if USE_DYNAMIC_MEMORY
+ #include <stdlib.h>
+#endif
+
+/**
+ * \addtogroup partition Partition table support
+ *
+ * Support for reading partition tables and access to partitions.
+ *
+ * @{
+ */
+/**
+ * \file
+ * Partition table implementation (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+/**
+ * \addtogroup partition_config Configuration of partition table support
+ * Preprocessor defines to configure the partition support.
+ */
+
+#if !USE_DYNAMIC_MEMORY
+static struct partition_struct partition_handles[PARTITION_COUNT];
+#endif
+
+/**
+ * Opens a partition.
+ *
+ * Opens a partition by its index number and returns a partition
+ * handle which describes the opened partition.
+ *
+ * \note This function does not support extended partitions.
+ *
+ * \param[in] device_read A function pointer which is used to read from the disk.
+ * \param[in] device_read_interval A function pointer which is used to read in constant intervals from the disk.
+ * \param[in] device_write A function pointer which is used to write to the disk.
+ * \param[in] device_write_interval A function pointer which is used to write a data stream to disk.
+ * \param[in] index The index of the partition which should be opened, range 0 to 3.
+ * A negative value is allowed as well. In this case, the partition opened is
+ * not checked for existance, begins at offset zero, has a length of zero
+ * and is of an unknown type. Use this in case you want to open the whole device
+ * as a single partition (e.g. for "super floppy" use).
+ * \returns 0 on failure, a partition descriptor on success.
+ * \see partition_close
+ */
+struct partition_struct* partition_open(device_read_t device_read, device_read_interval_t device_read_interval, device_write_t device_write, device_write_interval_t device_write_interval, int8_t index)
+{
+ struct partition_struct* new_partition = 0;
+ uint8_t buffer[0x10];
+
+ if(!device_read || !device_read_interval || index >= 4)
+ return 0;
+
+ if(index >= 0)
+ {
+ /* read specified partition table index */
+ if(!device_read(0x01be + index * 0x10, buffer, sizeof(buffer)))
+ return 0;
+
+ /* abort on empty partition entry */
+ if(buffer[4] == 0x00)
+ return 0;
+ }
+
+ /* allocate partition descriptor */
+#if USE_DYNAMIC_MEMORY
+ new_partition = malloc(sizeof(*new_partition));
+ if(!new_partition)
+ return 0;
+#else
+ new_partition = partition_handles;
+ uint8_t i;
+ for(i = 0; i < PARTITION_COUNT; ++i)
+ {
+ if(new_partition->type == PARTITION_TYPE_FREE)
+ break;
+
+ ++new_partition;
+ }
+ if(i >= PARTITION_COUNT)
+ return 0;
+#endif
+
+ memset(new_partition, 0, sizeof(*new_partition));
+
+ /* fill partition descriptor */
+ new_partition->device_read = device_read;
+ new_partition->device_read_interval = device_read_interval;
+ new_partition->device_write = device_write;
+ new_partition->device_write_interval = device_write_interval;
+
+ if(index >= 0)
+ {
+ new_partition->type = buffer[4];
+ new_partition->offset = read32(&buffer[8]);
+ new_partition->length = read32(&buffer[12]);
+ }
+ else
+ {
+ new_partition->type = 0xff;
+ }
+
+ return new_partition;
+}
+
+/**
+ * Closes a partition.
+ *
+ * This function destroys a partition descriptor which was
+ * previously obtained from a call to partition_open().
+ * When this function returns, the given descriptor will be
+ * invalid.
+ *
+ * \param[in] partition The partition descriptor to destroy.
+ * \returns 0 on failure, 1 on success.
+ * \see partition_open
+ */
+uint8_t partition_close(struct partition_struct* partition)
+{
+ if(!partition)
+ return 0;
+
+ /* destroy partition descriptor */
+#if USE_DYNAMIC_MEMORY
+ free(partition);
+#else
+ partition->type = PARTITION_TYPE_FREE;
+#endif
+
+ return 1;
+}
+
+/**
+ * @}
+ */
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef PARTITION_H
+#define PARTITION_H
+
+#include <stdint.h>
+#include "sd_raw_config.h"
+#include "partition_config.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup partition
+ *
+ * @{
+ */
+/**
+ * \file
+ * Partition table header (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+/**
+ * The partition table entry is not used.
+ */
+#define PARTITION_TYPE_FREE 0x00
+/**
+ * The partition contains a FAT12 filesystem.
+ */
+#define PARTITION_TYPE_FAT12 0x01
+/**
+ * The partition contains a FAT16 filesystem with 32MB maximum.
+ */
+#define PARTITION_TYPE_FAT16_32MB 0x04
+/**
+ * The partition is an extended partition with its own partition table.
+ */
+#define PARTITION_TYPE_EXTENDED 0x05
+/**
+ * The partition contains a FAT16 filesystem.
+ */
+#define PARTITION_TYPE_FAT16 0x06
+/**
+ * The partition contains a FAT32 filesystem.
+ */
+#define PARTITION_TYPE_FAT32 0x0b
+/**
+ * The partition contains a FAT32 filesystem with LBA.
+ */
+#define PARTITION_TYPE_FAT32_LBA 0x0c
+/**
+ * The partition contains a FAT16 filesystem with LBA.
+ */
+#define PARTITION_TYPE_FAT16_LBA 0x0e
+/**
+ * The partition is an extended partition with LBA.
+ */
+#define PARTITION_TYPE_EXTENDED_LBA 0x0f
+/**
+ * The partition has an unknown type.
+ */
+#define PARTITION_TYPE_UNKNOWN 0xff
+
+/**
+ * A function pointer used to read from the partition.
+ *
+ * \param[in] offset The offset on the device where to start reading.
+ * \param[out] buffer The buffer into which to place the data.
+ * \param[in] length The count of bytes to read.
+ */
+typedef uint8_t (*device_read_t)(offset_t offset, uint8_t* buffer, uintptr_t length);
+/**
+ * A function pointer passed to a \c device_read_interval_t.
+ *
+ * \param[in] buffer The buffer which contains the data just read.
+ * \param[in] offset The offset from which the data in \c buffer was read.
+ * \param[in] p An opaque pointer.
+ * \see device_read_interval_t
+ */
+typedef uint8_t (*device_read_callback_t)(uint8_t* buffer, offset_t offset, void* p);
+/**
+ * A function pointer used to continuously read units of \c interval bytes
+ * and call a callback function.
+ *
+ * This function starts reading at the specified offset. Every \c interval bytes,
+ * it calls the callback function with the associated data buffer.
+ *
+ * By returning zero, the callback may stop reading.
+ *
+ * \param[in] offset Offset from which to start reading.
+ * \param[in] buffer Pointer to a buffer which is at least interval bytes in size.
+ * \param[in] interval Number of bytes to read before calling the callback function.
+ * \param[in] length Number of bytes to read altogether.
+ * \param[in] callback The function to call every interval bytes.
+ * \param[in] p An opaque pointer directly passed to the callback function.
+ * \returns 0 on failure, 1 on success
+ * \see device_read_t
+ */
+typedef uint8_t (*device_read_interval_t)(offset_t offset, uint8_t* buffer, uintptr_t interval, uintptr_t length, device_read_callback_t callback, void* p);
+/**
+ * A function pointer used to write to the partition.
+ *
+ * \param[in] offset The offset on the device where to start writing.
+ * \param[in] buffer The buffer which to write.
+ * \param[in] length The count of bytes to write.
+ */
+typedef uint8_t (*device_write_t)(offset_t offset, const uint8_t* buffer, uintptr_t length);
+/**
+ * A function pointer passed to a \c device_write_interval_t.
+ *
+ * \param[in] buffer The buffer which receives the data to write.
+ * \param[in] offset The offset to which the data in \c buffer will be written.
+ * \param[in] p An opaque pointer.
+ * \returns The number of bytes put into \c buffer
+ * \see device_write_interval_t
+ */
+typedef uintptr_t (*device_write_callback_t)(uint8_t* buffer, offset_t offset, void* p);
+/**
+ * A function pointer used to continuously write a data stream obtained from
+ * a callback function.
+ *
+ * This function starts writing at the specified offset. To obtain the
+ * next bytes to write, it calls the callback function. The callback fills the
+ * provided data buffer and returns the number of bytes it has put into the buffer.
+ *
+ * By returning zero, the callback may stop writing.
+ *
+ * \param[in] offset Offset where to start writing.
+ * \param[in] buffer Pointer to a buffer which is used for the callback function.
+ * \param[in] length Number of bytes to write in total. May be zero for endless writes.
+ * \param[in] callback The function used to obtain the bytes to write.
+ * \param[in] p An opaque pointer directly passed to the callback function.
+ * \returns 0 on failure, 1 on success
+ * \see device_write_t
+ */
+typedef uint8_t (*device_write_interval_t)(offset_t offset, uint8_t* buffer, uintptr_t length, device_write_callback_t callback, void* p);
+
+/**
+ * Describes a partition.
+ */
+struct partition_struct
+{
+ /**
+ * The function which reads data from the partition.
+ *
+ * \note The offset given to this function is relative to the whole disk,
+ * not to the start of the partition.
+ */
+ device_read_t device_read;
+ /**
+ * The function which repeatedly reads a constant amount of data from the partition.
+ *
+ * \note The offset given to this function is relative to the whole disk,
+ * not to the start of the partition.
+ */
+ device_read_interval_t device_read_interval;
+ /**
+ * The function which writes data to the partition.
+ *
+ * \note The offset given to this function is relative to the whole disk,
+ * not to the start of the partition.
+ */
+ device_write_t device_write;
+ /**
+ * The function which repeatedly writes data to the partition.
+ *
+ * \note The offset given to this function is relative to the whole disk,
+ * not to the start of the partition.
+ */
+ device_write_interval_t device_write_interval;
+
+ /**
+ * The type of the partition.
+ *
+ * Compare this value to the PARTITION_TYPE_* constants.
+ */
+ uint8_t type;
+ /**
+ * The offset in blocks on the disk where this partition starts.
+ */
+ uint32_t offset;
+ /**
+ * The length in blocks of this partition.
+ */
+ uint32_t length;
+};
+
+struct partition_struct* partition_open(device_read_t device_read, device_read_interval_t device_read_interval, device_write_t device_write, device_write_interval_t device_write_interval, int8_t index);
+uint8_t partition_close(struct partition_struct* partition);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef PARTITION_CONFIG_H
+#define PARTITION_CONFIG_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup partition
+ *
+ * @{
+ */
+/**
+ * \file
+ * Partition configuration (license: GPLv2 or LGPLv2.1)
+ */
+
+/**
+ * \ingroup partition_config
+ * Maximum number of partition handles.
+ */
+#define PARTITION_COUNT 1
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: scheduler_config.h,v 1.1.2.1 2007-12-06 08:58:00 zer0 Exp $
+ *
+ */
+
+#ifndef _SCHEDULER_CONFIG_H_
+#define _SCHEDULER_CONFIG_H_
+
+#define _SCHEDULER_CONFIG_VERSION_ 4
+
+/** maximum number of allocated events */
+#define SCHEDULER_NB_MAX_EVENT 5
+
+
+/* define it only if CONFIG_MODULE_SCHEDULER_USE_TIMERS is enabled. In
+ this case, precaler is defined in timers_config.h in your project
+ directory. */
+#ifdef CONFIG_MODULE_SCHEDULER_USE_TIMERS
+/** the num of the timer to use for the scheduler */
+#define SCHEDULER_TIMER_NUM 0
+
+/* or set the prescaler manually (in this case, you use must TIMER0,
+ and the prescaler must be a correct value regarding the AVR device
+ you are using (look in include/aversive/parts.h). Obviously, the
+ values of SCHEDULER_CK and SCHEDULER_CLOCK_PRESCALER must also be
+ coherent (TIMER0_PRESCALER_DIV_VALUE and VALUE) */
+#endif /* CONFIG_MODULE_SCHEDULER_USE_TIMERS */
+
+
+#ifdef CONFIG_MODULE_SCHEDULER_TIMER0
+/* The 2 values below MUST be coherent:
+ * if SCHEDULER_CK = TIMER0_PRESCALER_DIV_x, then
+ * you must have SCHEDULER_CLOCK_PRESCALER = x too !!! */
+#define SCHEDULER_CK TIMER0_PRESCALER_DIV_8
+#define SCHEDULER_CLOCK_PRESCALER 8
+
+#endif /* CONFIG_MODULE_SCHEDULER_TIMER0 */
+
+/* last case, the scheduler is called manually. The user has to
+ define the period here */
+#ifdef CONFIG_MODULE_SCHEDULER_MANUAL
+
+#define SCHEDULER_UNIT_FLOAT 1024.0
+#define SCHEDULER_UNIT 1024UL
+
+#endif /* CONFIG_MODULE_SCHEDULER_MANUAL */
+
+/** number of allowed imbricated scheduler interrupts. The maximum
+ * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate
+ * more than once per event. If it is less, it can avoid to browse the
+ * event table, events are delayed (we loose precision) but it takes
+ * less CPU */
+#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT
+
+/** define it for debug infos (not recommended, because very slow on
+ * an AVR, it uses printf in an interrupt). It can be useful if
+ * prescaler is very high, making the timer interrupt period very
+ * long in comparison to printf() */
+/* #define SCHEDULER_DEBUG */
+
+#endif // _SCHEDULER_CONFIG_H_
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SD_READER_CONFIG_H
+#define SD_READER_CONFIG_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup config Sd-reader configuration
+ *
+ * @{
+ */
+
+/**
+ * \file
+ * Common sd-reader configuration used by all modules (license: GPLv2 or LGPLv2.1)
+ *
+ * \note This file contains only configuration items relevant to
+ * all sd-reader implementation files. For module specific configuration
+ * options, please see the files fat_config.h, partition_config.h
+ * and sd_raw_config.h.
+ */
+
+/**
+ * Controls allocation of memory.
+ *
+ * Set to 1 to use malloc()/free() for allocation of structures
+ * like file and directory handles, set to 0 to use pre-allocated
+ * fixed-size handle arrays.
+ */
+#define USE_DYNAMIC_MEMORY 0
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <aversive/pgmspace.h>
+
+#include "fat.h"
+#include "fat_config.h"
+#include "partition.h"
+#include "sd_raw.h"
+#include "sd_raw_config.h"
+#include "sd_log.h"
+
+static struct fat_file_struct *log_fd = NULL;
+
+static uint8_t find_file_in_dir(struct fat_fs_struct* fs,
+ struct fat_dir_struct* dd, const char* name,
+ struct fat_dir_entry_struct* dir_entry)
+{
+ (void)fs;
+
+ while(fat_read_dir(dd, dir_entry)) {
+ if(strcmp(dir_entry->long_name, name) == 0) {
+ fat_reset_dir(dd);
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+static struct fat_file_struct *open_file_in_dir(struct fat_fs_struct* fs,
+ struct fat_dir_struct* dd, const char* name)
+{
+ struct fat_dir_entry_struct file_entry;
+
+ if(!find_file_in_dir(fs, dd, name, &file_entry))
+ return 0;
+
+ return fat_open_file(fs, &file_entry);
+}
+
+/* open the log file on the SD card */
+int8_t sd_log_open(void)
+{
+ struct fat_file_struct *fd;
+ struct fat_fs_struct *fs;
+ struct partition_struct *partition ;
+ struct fat_dir_struct *dd;
+ struct fat_dir_entry_struct directory;
+ struct fat_dir_entry_struct file_entry;
+ int16_t i = 0;
+ char name[16];
+
+ /* setup sd card slot */
+ if (!sd_raw_init()) {
+#if SD_DEBUG
+ printf_P(PSTR("MMC/SD initialization failed\n"));
+#endif
+ return -1;
+ }
+
+ /* open first partition */
+ partition = partition_open(sd_raw_read,
+ sd_raw_read_interval,
+#if SD_RAW_WRITE_SUPPORT
+ sd_raw_write, sd_raw_write_interval,
+#else
+ 0, 0,
+#endif
+ 0);
+
+ if (!partition) {
+ /* If the partition did not open, assume the storage device
+ * is a "superfloppy", i.e. has no MBR.
+ */
+ partition = partition_open(sd_raw_read,
+ sd_raw_read_interval,
+#if SD_RAW_WRITE_SUPPORT
+ sd_raw_write,
+ sd_raw_write_interval,
+#else
+ 0,
+ 0,
+#endif
+ -1);
+ if (!partition) {
+#if SD_DEBUG
+ printf_P(PSTR("opening partition failed\n"));
+#endif
+ return -1;
+ }
+ }
+
+ /* open file system */
+ fs = fat_open(partition);
+ if (!fs) {
+#if SD_DEBUG
+ printf_P(PSTR("opening filesystem failed\n"));
+#endif
+ return -1;
+ }
+
+ /* open root directory */
+ fat_get_dir_entry_of_path(fs, "/", &directory);
+ dd = fat_open_dir(fs, &directory);
+ if (!dd) {
+#if SD_DEBUG
+ printf_P(PSTR("opening root directory failed\n"));
+#endif
+ return -1;
+ }
+
+ printf("choose log file name\n");
+ while (1) {
+ snprintf(name, sizeof(name), "log%.4d", i++);
+ if (!find_file_in_dir(fs, dd, name, &file_entry))
+ break;
+ }
+
+ printf("create log file %s\n", name);
+ if (!fat_create_file(dd, name, &file_entry)) {
+ printf_P(PSTR("error creating file: "));
+ }
+
+ fd = open_file_in_dir(fs, dd, name);
+ if (!fd) {
+ printf_P(PSTR("error opening "));
+ return -1;
+ }
+
+ return 0;
+}
+
+/* log output */
+intptr_t sd_log_write(const void *buffer, uintptr_t buffer_len)
+{
+ if (log_fd == NULL)
+ return -1;
+
+ return fat_write_file(log_fd, buffer, buffer_len);
+}
+
+uint8_t sd_log_enabled(void)
+{
+ return log_fd != NULL;
+}
--- /dev/null
+#ifndef SD_LOG_H_
+#define SD_LOG_H_
+
+/**
+ * open a log file on sd card.
+ *
+ * The format of the filename is "log%.4d". The choosen integer is the
+ * first file that doesn't exist on the sd card, starting from 0.
+ *
+ * @return
+ * 0 on success, negative value on error
+ */
+int8_t sd_log_open(void);
+
+/**
+ * Write a buffer in the log file
+ *
+ * This function fails (return -1)if the log file was not opened.
+ *
+ * @return
+ * number of written bytes on success, negative value en error
+ */
+intptr_t sd_log_write(const void *buffer, uintptr_t buffer_len);
+
+/**
+ * return true if log file was properly opened
+ */
+uint8_t sd_log_enabled(void);
+
+#endif
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 as
+ * published by the Free Software Foundation.
+ */
+
+#include <string.h>
+#include <avr/pgmspace.h>
+#include <avr/sleep.h>
+#include "fat.h"
+#include "fat_config.h"
+#include "partition.h"
+#include "sd_raw.h"
+#include "sd_raw_config.h"
+
+#include <uart.h>
+#include <stdio.h>
+#include <aversive/error.h>
+#include "cmdline.h"
+
+
+#define SD_DEBUG 1
+
+/**
+ * \mainpage MMC/SD/SDHC card library
+ *
+ * This project provides a general purpose library which implements read and write
+ * support for MMC, SD and SDHC memory cards.
+ *
+ * It includes
+ * - low-level \link sd_raw MMC, SD and SDHC read/write routines \endlink
+ * - \link partition partition table support \endlink
+ * - a simple \link fat FAT16/FAT32 read/write implementation \endlink
+ *
+ * \section circuit The circuit
+ * The circuit which was mainly used during development consists of an Atmel AVR
+ * microcontroller with some passive components. It is quite simple and provides
+ * an easy test environment. The circuit which can be downloaded on the
+ * <a href="http://www.roland-riegel.de/sd-reader/">project homepage</a> has been
+ * improved with regard to operation stability.
+ *
+ * I used different microcontrollers during development, the ATmega8 with 8kBytes
+ * of flash, and its pin-compatible alternative, the ATmega168 with 16kBytes flash.
+ * The first one is the one I started with, but when I implemented FAT16 write
+ * support, I ran out of flash space and switched to the ATmega168. For FAT32, an
+ * ATmega328 is required.
+ *
+ * The circuit board is a self-made and self-soldered board consisting of a single
+ * copper layer and standard DIL components, except of the MMC/SD card connector.
+ *
+ * The connector is soldered to the bottom side of the board. It has a simple
+ * eject button which, when a card is inserted, needs some space beyond the connector
+ * itself. As an additional feature the connector has two electrical switches
+ * to detect wether a card is inserted and wether this card is write-protected.
+ *
+ * \section pictures Pictures
+ * \image html pic01.jpg "The circuit board used to implement and test this application."
+ * \image html pic02.jpg "The MMC/SD card connector on the soldering side of the circuit board."
+ *
+ * \section software The software
+ * The software is written in C (ISO C99). It might not be the smallest or
+ * the fastest one, but I think it is quite flexible. See the project's
+ * <a href="http://www.roland-riegel.de/sd-reader/benchmarks/">benchmark page</a> to get an
+ * idea of the possible data rates.
+ *
+ * I implemented an example application providing a simple command prompt which is accessible
+ * via the UART at 9600 Baud. With commands similiar to the Unix shell you can browse different
+ * directories, read and write files, create new ones and delete them again. Not all commands are
+ * available in all software configurations.
+ * - <tt>cat \<file\></tt>\n
+ * Writes a hexdump of \<file\> to the terminal.
+ * - <tt>cd \<directory\></tt>\n
+ * Changes current working directory to \<directory\>.
+ * - <tt>disk</tt>\n
+ * Shows card manufacturer, status, filesystem capacity and free storage space.
+ * - <tt>init</tt>\n
+ * Reinitializes and reopens the memory card.
+ * - <tt>ls</tt>\n
+ * Shows the content of the current directory.
+ * - <tt>mkdir \<directory\></tt>\n
+ * Creates a directory called \<directory\>.
+ * - <tt>mv \<file\> \<file_new\></tt>\n
+ * Renames \<file\> to \<file_new\>.
+ * - <tt>rm \<file\></tt>\n
+ * Deletes \<file\>.
+ * - <tt>sync</tt>\n
+ * Ensures all buffered data is written to the card.
+ * - <tt>touch \<file\></tt>\n
+ * Creates \<file\>.
+ * - <tt>write \<file\> \<offset\></tt>\n
+ * Writes text to \<file\>, starting from \<offset\>. The text is read
+ * from the UART, line by line. Finish with an empty line.
+ *
+ * \htmlonly
+ * <p>
+ * The following table shows some typical code sizes in bytes, using the 20090330 release with a
+ * buffered read-write MMC/SD configuration, FAT16 and static memory allocation:
+ * </p>
+ *
+ * <table border="1" cellpadding="2">
+ * <tr>
+ * <th>layer</th>
+ * <th>code size</th>
+ * <th>static RAM usage</th>
+ * </tr>
+ * <tr>
+ * <td>MMC/SD</td>
+ * <td align="right">2410</td>
+ * <td align="right">518</td>
+ * </tr>
+ * <tr>
+ * <td>Partition</td>
+ * <td align="right">456</td>
+ * <td align="right">17</td>
+ * </tr>
+ * <tr>
+ * <td>FAT16</td>
+ * <td align="right">7928</td>
+ * <td align="right">188</td>
+ * </tr>
+ * </table>
+ *
+ * <p>
+ * The static RAM is mostly used for buffering memory card access, which
+ * improves performance and reduces implementation complexity.
+ * </p>
+ *
+ * <p>
+ * Please note that the numbers above do not include the C library functions
+ * used, e.g. some string functions. These will raise the numbers somewhat
+ * if they are not already used in other program parts.
+ * </p>
+ *
+ * <p>
+ * When opening a partition, filesystem, file or directory, a little amount
+ * of RAM is used, as listed in the following table. Depending on the library
+ * configuration, the memory is either allocated statically or dynamically.
+ * </p>
+ *
+ * <table border="1" cellpadding="2">
+ * <tr>
+ * <th>descriptor</th>
+ * <th>dynamic/static RAM</th>
+ * </tr>
+ * <tr>
+ * <td>partition</td>
+ * <td align="right">17</td>
+ * </tr>
+ * <tr>
+ * <td>filesystem</td>
+ * <td align="right">26</td>
+ * </tr>
+ * <tr>
+ * <td>file</td>
+ * <td align="right">53</td>
+ * </tr>
+ * <tr>
+ * <td>directory</td>
+ * <td align="right">49</td>
+ * </tr>
+ * </table>
+ *
+ * \endhtmlonly
+ *
+ * \section adaptation Adapting the software to your needs
+ * The only hardware dependent part is the communication layer talking to the
+ * memory card. The other parts like partition table and FAT support are
+ * completely independent, you could use them even for managing Compact Flash
+ * cards or standard ATAPI hard disks.
+ *
+ * By changing the MCU* variables in the Makefile, you can use other Atmel
+ * microcontrollers or different clock speeds. You might also want to change
+ * the configuration defines in the files fat_config.h, partition_config.h,
+ * sd_raw_config.h and sd-reader_config.h. For example, you could disable
+ * write support completely if you only need read support.
+ *
+ * For further information, visit the project's
+ * <a href="http://www.roland-riegel.de/sd-reader/faq/">FAQ page</a>.
+ *
+ * \section bugs Bugs or comments?
+ * If you have comments or found a bug in the software - there might be some
+ * of them - you may contact me per mail at feedback@roland-riegel.de.
+ *
+ * \section acknowledgements Acknowledgements
+ * Thanks go to Ulrich Radig, who explained on his homepage how to interface
+ * MMC cards to the Atmel microcontroller (http://www.ulrichradig.de/).
+ * I adapted his work for my circuit.
+ *
+ * \section copyright Copyright 2006-2012 by Roland Riegel
+ * This program is free software; you can redistribute it and/or modify it under
+ * the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation (http://www.gnu.org/copyleft/gpl.html).
+ * At your option, you can alternatively redistribute and/or modify the following
+ * files under the terms of the GNU Lesser General Public License version 2.1
+ * as published by the Free Software Foundation (http://www.gnu.org/copyleft/lgpl.html):
+ * - byteordering.c
+ * - byteordering.h
+ * - fat.c
+ * - fat.h
+ * - fat_config.h
+ * - partition.c
+ * - partition.h
+ * - partition_config.h
+ * - sd_raw.c
+ * - sd_raw.h
+ * - sd_raw_config.h
+ * - sd-reader_config.h
+ */
+
+static uint8_t read_line(char* buffer, uint8_t buffer_length);
+static uint32_t strtolong(const char* str);
+static uint8_t find_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name, struct fat_dir_entry_struct* dir_entry);
+static struct fat_file_struct* open_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name);
+static uint8_t print_disk_info(const struct fat_fs_struct* fs);
+
+int sd_main(void)
+{
+ /* we will just use ordinary idle mode */
+ set_sleep_mode(SLEEP_MODE_IDLE);
+
+ while(1)
+ {
+ printf_P(PSTR("init\n"));
+ /* setup sd card slot */
+ if(!sd_raw_init())
+ {
+#if SD_DEBUG
+ printf_P(PSTR("MMC/SD initialization failed\n"));
+#endif
+ continue;
+ }
+
+ /* open first partition */
+ struct partition_struct* partition = partition_open(sd_raw_read,
+ sd_raw_read_interval,
+#if SD_RAW_WRITE_SUPPORT
+ sd_raw_write,
+ sd_raw_write_interval,
+#else
+ 0,
+ 0,
+#endif
+ 0
+ );
+
+ if(!partition)
+ {
+ /* If the partition did not open, assume the storage device
+ * is a "superfloppy", i.e. has no MBR.
+ */
+ partition = partition_open(sd_raw_read,
+ sd_raw_read_interval,
+#if SD_RAW_WRITE_SUPPORT
+ sd_raw_write,
+ sd_raw_write_interval,
+#else
+ 0,
+ 0,
+#endif
+ -1
+ );
+ if(!partition)
+ {
+#if SD_DEBUG
+ printf_P(PSTR("opening partition failed\n"));
+#endif
+ continue;
+ }
+ }
+
+ /* open file system */
+ struct fat_fs_struct* fs = fat_open(partition);
+ if(!fs)
+ {
+#if SD_DEBUG
+ printf_P(PSTR("opening filesystem failed\n"));
+#endif
+ continue;
+ }
+
+ /* open root directory */
+ struct fat_dir_entry_struct directory;
+ fat_get_dir_entry_of_path(fs, "/", &directory);
+
+ struct fat_dir_struct* dd = fat_open_dir(fs, &directory);
+ if(!dd)
+ {
+#if SD_DEBUG
+ printf_P(PSTR("opening root directory failed\n"));
+#endif
+ continue;
+ }
+
+ /* print some card information as a boot message */
+ print_disk_info(fs);
+
+ /* provide a simple shell */
+ char buffer[24];
+ while(1)
+ {
+ /* print prompt */
+ printf_P(PSTR("> "));
+
+ /* read command */
+ char* command = buffer;
+ if(read_line(command, sizeof(buffer)) < 1)
+ continue;
+
+ /* execute command */
+ if(strcmp_P(command, PSTR("init")) == 0)
+ {
+ break;
+ }
+ else if(strncmp_P(command, PSTR("cd "), 3) == 0)
+ {
+ command += 3;
+ if(command[0] == '\0')
+ continue;
+
+ /* change directory */
+ struct fat_dir_entry_struct subdir_entry;
+ if(find_file_in_dir(fs, dd, command, &subdir_entry))
+ {
+ struct fat_dir_struct* dd_new = fat_open_dir(fs, &subdir_entry);
+ if(dd_new)
+ {
+ fat_close_dir(dd);
+ dd = dd_new;
+ continue;
+ }
+ }
+
+ printf_P(PSTR("directory not found: %s\n"), command);
+ }
+ else if(strcmp_P(command, PSTR("ls")) == 0)
+ {
+ /* print directory listing */
+ struct fat_dir_entry_struct dir_entry;
+ while(fat_read_dir(dd, &dir_entry))
+ {
+ uint8_t spaces = sizeof(dir_entry.long_name) - strlen(dir_entry.long_name) + 4;
+
+ printf_P(PSTR("%s%c"), dir_entry.long_name,
+ dir_entry.attributes & FAT_ATTRIB_DIR ? '/' : ' ');
+ while(spaces--)
+ uart_send(CMDLINE_UART, ' ');
+ printf_P(PSTR("%d\n"), dir_entry.file_size);
+ }
+ }
+ else if(strncmp_P(command, PSTR("cat "), 4) == 0)
+ {
+ command += 4;
+ if(command[0] == '\0')
+ continue;
+
+ /* search file in current directory and open it */
+ struct fat_file_struct* fd = open_file_in_dir(fs, dd, command);
+ if(!fd)
+ {
+ printf_P(PSTR("error opening: %s\n"), command);
+ continue;
+ }
+
+ /* print file contents */
+ uint8_t buffer[8];
+ uint32_t offset = 0;
+ intptr_t count;
+ while((count = fat_read_file(fd, buffer, sizeof(buffer))) > 0)
+ {
+ printf_P(PSTR("%"PRIu32":"), offset);
+ for(intptr_t i = 0; i < count; ++i)
+ {
+ printf_P(PSTR(" %x"), buffer[i]);
+ }
+ printf_P(PSTR("\n"));
+ offset += 8;
+ }
+
+ fat_close_file(fd);
+ }
+ else if(strcmp_P(command, PSTR("disk")) == 0)
+ {
+ if(!print_disk_info(fs))
+ printf_P(PSTR("error reading disk info\n"));
+ }
+#if FAT_WRITE_SUPPORT
+ else if(strncmp_P(command, PSTR("rm "), 3) == 0)
+ {
+ command += 3;
+ if(command[0] == '\0')
+ continue;
+
+ struct fat_dir_entry_struct file_entry;
+ if(find_file_in_dir(fs, dd, command, &file_entry))
+ {
+ if(fat_delete_file(fs, &file_entry))
+ continue;
+ }
+
+ printf_P(PSTR("error deleting file: %s\n"), command);
+ }
+ else if(strncmp_P(command, PSTR("touch "), 6) == 0)
+ {
+ command += 6;
+ if(command[0] == '\0')
+ continue;
+
+ struct fat_dir_entry_struct file_entry;
+ if(!fat_create_file(dd, command, &file_entry))
+ {
+ printf_P(PSTR("error creating file: %s\n"), command);
+ }
+ }
+ else if(strncmp_P(command, PSTR("mv "), 3) == 0)
+ {
+ command += 3;
+ if(command[0] == '\0')
+ continue;
+
+ char* target = command;
+ while(*target != ' ' && *target != '\0')
+ ++target;
+
+ if(*target == ' ')
+ *target++ = '\0';
+ else
+ continue;
+
+ struct fat_dir_entry_struct file_entry;
+ if(find_file_in_dir(fs, dd, command, &file_entry))
+ {
+ if(fat_move_file(fs, &file_entry, dd, target))
+ continue;
+ }
+
+ printf_P(PSTR("error moving file: %s\n"), command);
+ }
+ else if(strncmp_P(command, PSTR("write "), 6) == 0)
+ {
+ command += 6;
+ if(command[0] == '\0')
+ continue;
+
+ char* offset_value = command;
+ while(*offset_value != ' ' && *offset_value != '\0')
+ ++offset_value;
+
+ if(*offset_value == ' ')
+ *offset_value++ = '\0';
+ else
+ continue;
+
+ /* search file in current directory and open it */
+ struct fat_file_struct* fd = open_file_in_dir(fs, dd, command);
+ if(!fd)
+ {
+ printf_P(PSTR("error opening %s\n"), command);
+ continue;
+ }
+
+ int32_t offset = strtolong(offset_value);
+ if(!fat_seek_file(fd, &offset, FAT_SEEK_SET))
+ {
+ printf_P(PSTR("error seeking on %s\n"), command);
+
+ fat_close_file(fd);
+ continue;
+ }
+
+ /* read text from the shell and write it to the file */
+ uint8_t data_len;
+ while(1)
+ {
+ /* give a different prompt */
+ printf_P(PSTR("< "));
+
+ /* read one line of text */
+ data_len = read_line(buffer, sizeof(buffer));
+ if(!data_len)
+ break;
+
+ /* write text to file */
+ if(fat_write_file(fd, (uint8_t*) buffer, data_len) != data_len)
+ {
+ printf_P(PSTR("error writing to file\n"));
+ break;
+ }
+ }
+
+ fat_close_file(fd);
+ }
+ else if(strncmp_P(command, PSTR("mkdir "), 6) == 0)
+ {
+ command += 6;
+ if(command[0] == '\0')
+ continue;
+
+ struct fat_dir_entry_struct dir_entry;
+ if(!fat_create_dir(dd, command, &dir_entry))
+ {
+ printf_P(PSTR("error creating directory: %s\n"), command);
+ }
+ }
+#endif
+#if SD_RAW_WRITE_BUFFERING
+ else if(strcmp_P(command, PSTR("sync")) == 0)
+ {
+ if(!sd_raw_sync())
+ printf_P(PSTR("error syncing disk\n"));
+ }
+#endif
+ else
+ {
+ printf_P(PSTR("unknown command: %s\n"), command);
+ }
+ }
+
+ /* close directory */
+ fat_close_dir(dd);
+
+ /* close file system */
+ fat_close(fs);
+
+ /* close partition */
+ partition_close(partition);
+ }
+
+ return 0;
+}
+
+uint8_t uart_getc(void)
+{
+ uint8_t b = uart_recv(CMDLINE_UART);
+ if(b == '\r')
+ b = '\n';
+
+ return b;
+}
+
+uint8_t read_line(char* buffer, uint8_t buffer_length)
+{
+ memset(buffer, 0, buffer_length);
+
+ uint8_t read_length = 0;
+ while(read_length < buffer_length - 1)
+ {
+ uint8_t c = uart_getc();
+
+ if(c == 0x08 || c == 0x7f)
+ {
+ if(read_length < 1)
+ continue;
+
+ --read_length;
+ buffer[read_length] = '\0';
+
+ uart_send(CMDLINE_UART, 0x08);
+ uart_send(CMDLINE_UART, ' ');
+ uart_send(CMDLINE_UART, 0x08);
+
+ continue;
+ }
+
+ uart_send(CMDLINE_UART, c);
+
+ if(c == '\n')
+ {
+ buffer[read_length] = '\0';
+ break;
+ }
+ else
+ {
+ buffer[read_length] = c;
+ ++read_length;
+ }
+ }
+
+ return read_length;
+}
+
+uint32_t strtolong(const char* str)
+{
+ uint32_t l = 0;
+ while(*str >= '0' && *str <= '9')
+ l = l * 10 + (*str++ - '0');
+
+ return l;
+}
+
+uint8_t find_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name, struct fat_dir_entry_struct* dir_entry)
+{
+ (void)fs;
+
+ while(fat_read_dir(dd, dir_entry))
+ {
+ if(strcmp(dir_entry->long_name, name) == 0)
+ {
+ fat_reset_dir(dd);
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+struct fat_file_struct* open_file_in_dir(struct fat_fs_struct* fs, struct fat_dir_struct* dd, const char* name)
+{
+ struct fat_dir_entry_struct file_entry;
+ if(!find_file_in_dir(fs, dd, name, &file_entry))
+ return 0;
+
+ return fat_open_file(fs, &file_entry);
+}
+
+uint8_t print_disk_info(const struct fat_fs_struct* fs)
+{
+ if(!fs)
+ return 0;
+
+ struct sd_raw_info disk_info;
+ if(!sd_raw_get_info(&disk_info))
+ return 0;
+
+ printf_P(PSTR("manuf: 0x%x\n"), disk_info.manufacturer);
+ printf_P(PSTR("oem: %s\n"), disk_info.oem);
+ printf_P(PSTR("prod: %s\n"), disk_info.product);
+ printf_P(PSTR("rev: 0x%x\n"), disk_info.revision);
+ printf_P(PSTR("serial: 0x%x\n"), disk_info.serial);
+ printf_P(PSTR("date: 0x%d 0x%d\n"), disk_info.manufacturing_month,
+ disk_info.manufacturing_year);
+ printf_P(PSTR("size: %"PRIu32"\n"), disk_info.capacity / 1024 / 1024);
+ printf_P(PSTR("copy: 0x%x\n"), disk_info.flag_copy);
+ printf_P(PSTR("wr.pr.: 0x%x 0x%x\n"), disk_info.flag_write_protect_temp,
+ disk_info.flag_write_protect);
+ printf_P(PSTR("format: %d\n"), disk_info.format);
+ printf_P(PSTR("free: %d %d\n"), fat_get_fs_free(fs), fat_get_fs_size(fs));
+
+ return 1;
+}
+
+#if FAT_DATETIME_SUPPORT
+void get_datetime(uint16_t* year, uint8_t* month, uint8_t* day, uint8_t* hour, uint8_t* min, uint8_t* sec)
+{
+ *year = 2007;
+ *month = 1;
+ *day = 1;
+ *hour = 0;
+ *min = 0;
+ *sec = 0;
+}
+#endif
+
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#include <string.h>
+#include <avr/pgmspace.h>
+#include <avr/io.h>
+#include "sd_raw.h"
+
+#include <uart.h>
+
+/**
+ * \addtogroup sd_raw MMC/SD/SDHC card raw access
+ *
+ * This module implements read and write access to MMC, SD
+ * and SDHC cards. It serves as a low-level driver for the
+ * higher level modules such as partition and file system
+ * access.
+ *
+ * @{
+ */
+/**
+ * \file
+ * MMC/SD/SDHC raw access implementation (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+/**
+ * \addtogroup sd_raw_config MMC/SD configuration
+ * Preprocessor defines to configure the MMC/SD support.
+ */
+
+/**
+ * @}
+ */
+
+/* commands available in SPI mode */
+
+/* CMD0: response R1 */
+#define CMD_GO_IDLE_STATE 0x00
+/* CMD1: response R1 */
+#define CMD_SEND_OP_COND 0x01
+/* CMD8: response R7 */
+#define CMD_SEND_IF_COND 0x08
+/* CMD9: response R1 */
+#define CMD_SEND_CSD 0x09
+/* CMD10: response R1 */
+#define CMD_SEND_CID 0x0a
+/* CMD12: response R1 */
+#define CMD_STOP_TRANSMISSION 0x0c
+/* CMD13: response R2 */
+#define CMD_SEND_STATUS 0x0d
+/* CMD16: arg0[31:0]: block length, response R1 */
+#define CMD_SET_BLOCKLEN 0x10
+/* CMD17: arg0[31:0]: data address, response R1 */
+#define CMD_READ_SINGLE_BLOCK 0x11
+/* CMD18: arg0[31:0]: data address, response R1 */
+#define CMD_READ_MULTIPLE_BLOCK 0x12
+/* CMD24: arg0[31:0]: data address, response R1 */
+#define CMD_WRITE_SINGLE_BLOCK 0x18
+/* CMD25: arg0[31:0]: data address, response R1 */
+#define CMD_WRITE_MULTIPLE_BLOCK 0x19
+/* CMD27: response R1 */
+#define CMD_PROGRAM_CSD 0x1b
+/* CMD28: arg0[31:0]: data address, response R1b */
+#define CMD_SET_WRITE_PROT 0x1c
+/* CMD29: arg0[31:0]: data address, response R1b */
+#define CMD_CLR_WRITE_PROT 0x1d
+/* CMD30: arg0[31:0]: write protect data address, response R1 */
+#define CMD_SEND_WRITE_PROT 0x1e
+/* CMD32: arg0[31:0]: data address, response R1 */
+#define CMD_TAG_SECTOR_START 0x20
+/* CMD33: arg0[31:0]: data address, response R1 */
+#define CMD_TAG_SECTOR_END 0x21
+/* CMD34: arg0[31:0]: data address, response R1 */
+#define CMD_UNTAG_SECTOR 0x22
+/* CMD35: arg0[31:0]: data address, response R1 */
+#define CMD_TAG_ERASE_GROUP_START 0x23
+/* CMD36: arg0[31:0]: data address, response R1 */
+#define CMD_TAG_ERASE_GROUP_END 0x24
+/* CMD37: arg0[31:0]: data address, response R1 */
+#define CMD_UNTAG_ERASE_GROUP 0x25
+/* CMD38: arg0[31:0]: stuff bits, response R1b */
+#define CMD_ERASE 0x26
+/* ACMD41: arg0[31:0]: OCR contents, response R1 */
+#define CMD_SD_SEND_OP_COND 0x29
+/* CMD42: arg0[31:0]: stuff bits, response R1b */
+#define CMD_LOCK_UNLOCK 0x2a
+/* CMD55: arg0[31:0]: stuff bits, response R1 */
+#define CMD_APP 0x37
+/* CMD58: arg0[31:0]: stuff bits, response R3 */
+#define CMD_READ_OCR 0x3a
+/* CMD59: arg0[31:1]: stuff bits, arg0[0:0]: crc option, response R1 */
+#define CMD_CRC_ON_OFF 0x3b
+
+/* command responses */
+/* R1: size 1 byte */
+#define R1_IDLE_STATE 0
+#define R1_ERASE_RESET 1
+#define R1_ILL_COMMAND 2
+#define R1_COM_CRC_ERR 3
+#define R1_ERASE_SEQ_ERR 4
+#define R1_ADDR_ERR 5
+#define R1_PARAM_ERR 6
+/* R1b: equals R1, additional busy bytes */
+/* R2: size 2 bytes */
+#define R2_CARD_LOCKED 0
+#define R2_WP_ERASE_SKIP 1
+#define R2_ERR 2
+#define R2_CARD_ERR 3
+#define R2_CARD_ECC_FAIL 4
+#define R2_WP_VIOLATION 5
+#define R2_INVAL_ERASE 6
+#define R2_OUT_OF_RANGE 7
+#define R2_CSD_OVERWRITE 7
+#define R2_IDLE_STATE (R1_IDLE_STATE + 8)
+#define R2_ERASE_RESET (R1_ERASE_RESET + 8)
+#define R2_ILL_COMMAND (R1_ILL_COMMAND + 8)
+#define R2_COM_CRC_ERR (R1_COM_CRC_ERR + 8)
+#define R2_ERASE_SEQ_ERR (R1_ERASE_SEQ_ERR + 8)
+#define R2_ADDR_ERR (R1_ADDR_ERR + 8)
+#define R2_PARAM_ERR (R1_PARAM_ERR + 8)
+/* R3: size 5 bytes */
+#define R3_OCR_MASK (0xffffffffUL)
+#define R3_IDLE_STATE (R1_IDLE_STATE + 32)
+#define R3_ERASE_RESET (R1_ERASE_RESET + 32)
+#define R3_ILL_COMMAND (R1_ILL_COMMAND + 32)
+#define R3_COM_CRC_ERR (R1_COM_CRC_ERR + 32)
+#define R3_ERASE_SEQ_ERR (R1_ERASE_SEQ_ERR + 32)
+#define R3_ADDR_ERR (R1_ADDR_ERR + 32)
+#define R3_PARAM_ERR (R1_PARAM_ERR + 32)
+/* Data Response: size 1 byte */
+#define DR_STATUS_MASK 0x0e
+#define DR_STATUS_ACCEPTED 0x05
+#define DR_STATUS_CRC_ERR 0x0a
+#define DR_STATUS_WRITE_ERR 0x0c
+
+/* status bits for card types */
+#define SD_RAW_SPEC_1 0
+#define SD_RAW_SPEC_2 1
+#define SD_RAW_SPEC_SDHC 2
+
+#if !SD_RAW_SAVE_RAM
+/* static data buffer for acceleration */
+static uint8_t raw_block[512];
+/* offset where the data within raw_block lies on the card */
+static offset_t raw_block_address;
+#if SD_RAW_WRITE_BUFFERING
+/* flag to remember if raw_block was written to the card */
+static uint8_t raw_block_written;
+#endif
+#endif
+
+/* card type state */
+static uint8_t sd_raw_card_type;
+
+/* private helper functions */
+static void sd_raw_send_byte(uint8_t b);
+static uint8_t sd_raw_rec_byte(void);
+static uint8_t sd_raw_send_command(uint8_t command, uint32_t arg);
+
+/**
+ * \ingroup sd_raw
+ * Initializes memory card communication.
+ *
+ * \returns 0 on failure, 1 on success.
+ */
+uint8_t sd_raw_init(void)
+{
+ /* enable inputs for reading card status */
+ configure_pin_available();
+ configure_pin_locked();
+
+ /* enable outputs for MOSI, SCK, SS, input for MISO */
+ configure_pin_mosi();
+ configure_pin_sck();
+ configure_pin_ss();
+ configure_pin_miso();
+
+ unselect_card();
+
+ /* initialize SPI with lowest frequency; max. 400kHz during identification mode of card */
+ SPCR = (0 << SPIE) | /* SPI Interrupt Enable */
+ (1 << SPE) | /* SPI Enable */
+ (0 << DORD) | /* Data Order: MSB first */
+ (1 << MSTR) | /* Master mode */
+ (0 << CPOL) | /* Clock Polarity: SCK low when idle */
+ (0 << CPHA) | /* Clock Phase: sample on rising SCK edge */
+ (1 << SPR1) | /* Clock Frequency: f_OSC / 128 */
+ (1 << SPR0);
+ SPSR &= ~(1 << SPI2X); /* No doubled clock frequency */
+
+ /* initialization procedure */
+ sd_raw_card_type = 0;
+
+ if(!sd_raw_available())
+ return 0;
+
+ /* card needs 74 cycles minimum to start up */
+ for(uint8_t i = 0; i < 10; ++i)
+ {
+ /* wait 8 clock cycles */
+ sd_raw_rec_byte();
+ }
+
+ /* address card */
+ select_card();
+
+ /* reset card */
+ uint8_t response;
+ for(uint16_t i = 0; ; ++i)
+ {
+ response = sd_raw_send_command(CMD_GO_IDLE_STATE, 0);
+ if(response == (1 << R1_IDLE_STATE))
+ break;
+
+ if(i == 0x1ff)
+ {
+ unselect_card();
+ return 0;
+ }
+ }
+
+#if SD_RAW_SDHC
+ /* check for version of SD card specification */
+ response = sd_raw_send_command(CMD_SEND_IF_COND, 0x100 /* 2.7V - 3.6V */ | 0xaa /* test pattern */);
+ if((response & (1 << R1_ILL_COMMAND)) == 0)
+ {
+ sd_raw_rec_byte();
+ sd_raw_rec_byte();
+ if((sd_raw_rec_byte() & 0x01) == 0)
+ return 0; /* card operation voltage range doesn't match */
+ if(sd_raw_rec_byte() != 0xaa)
+ return 0; /* wrong test pattern */
+
+ /* card conforms to SD 2 card specification */
+ sd_raw_card_type |= (1 << SD_RAW_SPEC_2);
+ }
+ else
+#endif
+ {
+ /* determine SD/MMC card type */
+ sd_raw_send_command(CMD_APP, 0);
+ response = sd_raw_send_command(CMD_SD_SEND_OP_COND, 0);
+ if((response & (1 << R1_ILL_COMMAND)) == 0)
+ {
+ /* card conforms to SD 1 card specification */
+ sd_raw_card_type |= (1 << SD_RAW_SPEC_1);
+ }
+ else
+ {
+ /* MMC card */
+ }
+ }
+
+ /* wait for card to get ready */
+ for(uint16_t i = 0; ; ++i)
+ {
+ if(sd_raw_card_type & ((1 << SD_RAW_SPEC_1) | (1 << SD_RAW_SPEC_2)))
+ {
+ uint32_t arg = 0;
+#if SD_RAW_SDHC
+ if(sd_raw_card_type & (1 << SD_RAW_SPEC_2))
+ arg = 0x40000000;
+#endif
+ sd_raw_send_command(CMD_APP, 0);
+ response = sd_raw_send_command(CMD_SD_SEND_OP_COND, arg);
+ }
+ else
+ {
+ response = sd_raw_send_command(CMD_SEND_OP_COND, 0);
+ }
+
+ if((response & (1 << R1_IDLE_STATE)) == 0)
+ break;
+
+ if(i == 0x7fff)
+ {
+ unselect_card();
+ return 0;
+ }
+ }
+
+#if SD_RAW_SDHC
+ if(sd_raw_card_type & (1 << SD_RAW_SPEC_2))
+ {
+ if(sd_raw_send_command(CMD_READ_OCR, 0))
+ {
+ unselect_card();
+ return 0;
+ }
+
+ if(sd_raw_rec_byte() & 0x40)
+ sd_raw_card_type |= (1 << SD_RAW_SPEC_SDHC);
+
+ sd_raw_rec_byte();
+ sd_raw_rec_byte();
+ sd_raw_rec_byte();
+ }
+#endif
+
+ /* set block size to 512 bytes */
+ if(sd_raw_send_command(CMD_SET_BLOCKLEN, 512))
+ {
+ unselect_card();
+ return 0;
+ }
+
+ /* deaddress card */
+ unselect_card();
+
+ /* switch to highest SPI frequency possible */
+ SPCR &= ~((1 << SPR1) | (1 << SPR0)); /* Clock Frequency: f_OSC / 4 */
+ SPSR |= (1 << SPI2X); /* Doubled Clock Frequency: f_OSC / 2 */
+
+#if !SD_RAW_SAVE_RAM
+ /* the first block is likely to be accessed first, so precache it here */
+ raw_block_address = (offset_t) -1;
+#if SD_RAW_WRITE_BUFFERING
+ raw_block_written = 1;
+#endif
+ if(!sd_raw_read(0, raw_block, sizeof(raw_block)))
+ return 0;
+#endif
+
+ return 1;
+}
+
+/**
+ * \ingroup sd_raw
+ * Checks wether a memory card is located in the slot.
+ *
+ * \returns 1 if the card is available, 0 if it is not.
+ */
+uint8_t sd_raw_available()
+{
+ return get_pin_available() == 0x00;
+}
+
+/**
+ * \ingroup sd_raw
+ * Checks wether the memory card is locked for write access.
+ *
+ * \returns 1 if the card is locked, 0 if it is not.
+ */
+uint8_t sd_raw_locked()
+{
+ return get_pin_locked() == 0x00;
+}
+
+/**
+ * \ingroup sd_raw
+ * Sends a raw byte to the memory card.
+ *
+ * \param[in] b The byte to sent.
+ * \see sd_raw_rec_byte
+ */
+void sd_raw_send_byte(uint8_t b)
+{
+ SPDR = b;
+ /* wait for byte to be shifted out */
+ while(!(SPSR & (1 << SPIF)));
+ SPSR &= ~(1 << SPIF);
+}
+
+/**
+ * \ingroup sd_raw
+ * Receives a raw byte from the memory card.
+ *
+ * \returns The byte which should be read.
+ * \see sd_raw_send_byte
+ */
+uint8_t sd_raw_rec_byte()
+{
+ /* send dummy data for receiving some */
+ SPDR = 0xff;
+ while(!(SPSR & (1 << SPIF)));
+ SPSR &= ~(1 << SPIF);
+
+ return SPDR;
+}
+
+/**
+ * \ingroup sd_raw
+ * Send a command to the memory card which responses with a R1 response (and possibly others).
+ *
+ * \param[in] command The command to send.
+ * \param[in] arg The argument for command.
+ * \returns The command answer.
+ */
+uint8_t sd_raw_send_command(uint8_t command, uint32_t arg)
+{
+ uint8_t response;
+
+ /* wait some clock cycles */
+ sd_raw_rec_byte();
+
+ /* send command via SPI */
+ sd_raw_send_byte(0x40 | command);
+ sd_raw_send_byte((arg >> 24) & 0xff);
+ sd_raw_send_byte((arg >> 16) & 0xff);
+ sd_raw_send_byte((arg >> 8) & 0xff);
+ sd_raw_send_byte((arg >> 0) & 0xff);
+ switch(command)
+ {
+ case CMD_GO_IDLE_STATE:
+ sd_raw_send_byte(0x95);
+ break;
+ case CMD_SEND_IF_COND:
+ sd_raw_send_byte(0x87);
+ break;
+ default:
+ sd_raw_send_byte(0xff);
+ break;
+ }
+
+ /* receive response */
+ for(uint8_t i = 0; i < 10; ++i)
+ {
+ response = sd_raw_rec_byte();
+ if(response != 0xff)
+ break;
+ }
+
+ return response;
+}
+
+/**
+ * \ingroup sd_raw
+ * Reads raw data from the card.
+ *
+ * \param[in] offset The offset from which to read.
+ * \param[out] buffer The buffer into which to write the data.
+ * \param[in] length The number of bytes to read.
+ * \returns 0 on failure, 1 on success.
+ * \see sd_raw_read_interval, sd_raw_write, sd_raw_write_interval
+ */
+uint8_t sd_raw_read(offset_t offset, uint8_t* buffer, uintptr_t length)
+{
+ offset_t block_address;
+ uint16_t block_offset;
+ uint16_t read_length;
+ while(length > 0)
+ {
+ /* determine byte count to read at once */
+ block_offset = offset & 0x01ff;
+ block_address = offset - block_offset;
+ read_length = 512 - block_offset; /* read up to block border */
+ if(read_length > length)
+ read_length = length;
+
+#if !SD_RAW_SAVE_RAM
+ /* check if the requested data is cached */
+ if(block_address != raw_block_address)
+#endif
+ {
+#if SD_RAW_WRITE_BUFFERING
+ if(!sd_raw_sync())
+ return 0;
+#endif
+
+ /* address card */
+ select_card();
+
+ /* send single block request */
+#if SD_RAW_SDHC
+ if(sd_raw_send_command(CMD_READ_SINGLE_BLOCK, (sd_raw_card_type & (1 << SD_RAW_SPEC_SDHC) ? block_address / 512 : block_address)))
+#else
+ if(sd_raw_send_command(CMD_READ_SINGLE_BLOCK, block_address))
+#endif
+ {
+ unselect_card();
+ return 0;
+ }
+
+ /* wait for data block (start byte 0xfe) */
+ while(sd_raw_rec_byte() != 0xfe);
+
+#if SD_RAW_SAVE_RAM
+ /* read byte block */
+ uint16_t read_to = block_offset + read_length;
+ for(uint16_t i = 0; i < 512; ++i)
+ {
+ uint8_t b = sd_raw_rec_byte();
+ if(i >= block_offset && i < read_to)
+ *buffer++ = b;
+ }
+#else
+ /* read byte block */
+ uint8_t* cache = raw_block;
+ for(uint16_t i = 0; i < 512; ++i)
+ *cache++ = sd_raw_rec_byte();
+ raw_block_address = block_address;
+
+ memcpy(buffer, raw_block + block_offset, read_length);
+ buffer += read_length;
+#endif
+
+ /* read crc16 */
+ sd_raw_rec_byte();
+ sd_raw_rec_byte();
+
+ /* deaddress card */
+ unselect_card();
+
+ /* let card some time to finish */
+ sd_raw_rec_byte();
+ }
+#if !SD_RAW_SAVE_RAM
+ else
+ {
+ /* use cached data */
+ memcpy(buffer, raw_block + block_offset, read_length);
+ buffer += read_length;
+ }
+#endif
+
+ length -= read_length;
+ offset += read_length;
+ }
+
+ return 1;
+}
+
+/**
+ * \ingroup sd_raw
+ * Continuously reads units of \c interval bytes and calls a callback function.
+ *
+ * This function starts reading at the specified offset. Every \c interval bytes,
+ * it calls the callback function with the associated data buffer.
+ *
+ * By returning zero, the callback may stop reading.
+ *
+ * \note Within the callback function, you can not start another read or
+ * write operation.
+ * \note This function only works if the following conditions are met:
+ * - (offset - (offset % 512)) % interval == 0
+ * - length % interval == 0
+ *
+ * \param[in] offset Offset from which to start reading.
+ * \param[in] buffer Pointer to a buffer which is at least interval bytes in size.
+ * \param[in] interval Number of bytes to read before calling the callback function.
+ * \param[in] length Number of bytes to read altogether.
+ * \param[in] callback The function to call every interval bytes.
+ * \param[in] p An opaque pointer directly passed to the callback function.
+ * \returns 0 on failure, 1 on success
+ * \see sd_raw_write_interval, sd_raw_read, sd_raw_write
+ */
+uint8_t sd_raw_read_interval(offset_t offset, uint8_t* buffer, uintptr_t interval, uintptr_t length, sd_raw_read_interval_handler_t callback, void* p)
+{
+ if(!buffer || interval == 0 || length < interval || !callback)
+ return 0;
+
+#if !SD_RAW_SAVE_RAM
+ while(length >= interval)
+ {
+ /* as reading is now buffered, we directly
+ * hand over the request to sd_raw_read()
+ */
+ if(!sd_raw_read(offset, buffer, interval))
+ return 0;
+ if(!callback(buffer, offset, p))
+ break;
+ offset += interval;
+ length -= interval;
+ }
+
+ return 1;
+#else
+ /* address card */
+ select_card();
+
+ uint16_t block_offset;
+ uint16_t read_length;
+ uint8_t* buffer_cur;
+ uint8_t finished = 0;
+ do
+ {
+ /* determine byte count to read at once */
+ block_offset = offset & 0x01ff;
+ read_length = 512 - block_offset;
+
+ /* send single block request */
+#if SD_RAW_SDHC
+ if(sd_raw_send_command(CMD_READ_SINGLE_BLOCK, (sd_raw_card_type & (1 << SD_RAW_SPEC_SDHC) ? offset / 512 : offset - block_offset)))
+#else
+ if(sd_raw_send_command(CMD_READ_SINGLE_BLOCK, offset - block_offset))
+#endif
+ {
+ unselect_card();
+ return 0;
+ }
+
+ /* wait for data block (start byte 0xfe) */
+ while(sd_raw_rec_byte() != 0xfe);
+
+ /* read up to the data of interest */
+ for(uint16_t i = 0; i < block_offset; ++i)
+ sd_raw_rec_byte();
+
+ /* read interval bytes of data and execute the callback */
+ do
+ {
+ if(read_length < interval || length < interval)
+ break;
+
+ buffer_cur = buffer;
+ for(uint16_t i = 0; i < interval; ++i)
+ *buffer_cur++ = sd_raw_rec_byte();
+
+ if(!callback(buffer, offset + (512 - read_length), p))
+ {
+ finished = 1;
+ break;
+ }
+
+ read_length -= interval;
+ length -= interval;
+
+ } while(read_length > 0 && length > 0);
+
+ /* read rest of data block */
+ while(read_length-- > 0)
+ sd_raw_rec_byte();
+
+ /* read crc16 */
+ sd_raw_rec_byte();
+ sd_raw_rec_byte();
+
+ if(length < interval)
+ break;
+
+ offset = offset - block_offset + 512;
+
+ } while(!finished);
+
+ /* deaddress card */
+ unselect_card();
+
+ /* let card some time to finish */
+ sd_raw_rec_byte();
+
+ return 1;
+#endif
+}
+
+#if DOXYGEN || SD_RAW_WRITE_SUPPORT
+/**
+ * \ingroup sd_raw
+ * Writes raw data to the card.
+ *
+ * \note If write buffering is enabled, you might have to
+ * call sd_raw_sync() before disconnecting the card
+ * to ensure all remaining data has been written.
+ *
+ * \param[in] offset The offset where to start writing.
+ * \param[in] buffer The buffer containing the data to be written.
+ * \param[in] length The number of bytes to write.
+ * \returns 0 on failure, 1 on success.
+ * \see sd_raw_write_interval, sd_raw_read, sd_raw_read_interval
+ */
+uint8_t sd_raw_write(offset_t offset, const uint8_t* buffer, uintptr_t length)
+{
+ if(sd_raw_locked())
+ return 0;
+
+ offset_t block_address;
+ uint16_t block_offset;
+ uint16_t write_length;
+ while(length > 0)
+ {
+ /* determine byte count to write at once */
+ block_offset = offset & 0x01ff;
+ block_address = offset - block_offset;
+ write_length = 512 - block_offset; /* write up to block border */
+ if(write_length > length)
+ write_length = length;
+
+ /* Merge the data to write with the content of the block.
+ * Use the cached block if available.
+ */
+ if(block_address != raw_block_address)
+ {
+#if SD_RAW_WRITE_BUFFERING
+ if(!sd_raw_sync())
+ return 0;
+#endif
+
+ if(block_offset || write_length < 512)
+ {
+ if(!sd_raw_read(block_address, raw_block, sizeof(raw_block)))
+ return 0;
+ }
+ raw_block_address = block_address;
+ }
+
+ if(buffer != raw_block)
+ {
+ memcpy(raw_block + block_offset, buffer, write_length);
+
+#if SD_RAW_WRITE_BUFFERING
+ raw_block_written = 0;
+
+ if(length == write_length)
+ return 1;
+#endif
+ }
+
+ /* address card */
+ select_card();
+
+ /* send single block request */
+#if SD_RAW_SDHC
+ if(sd_raw_send_command(CMD_WRITE_SINGLE_BLOCK, (sd_raw_card_type & (1 << SD_RAW_SPEC_SDHC) ? block_address / 512 : block_address)))
+#else
+ if(sd_raw_send_command(CMD_WRITE_SINGLE_BLOCK, block_address))
+#endif
+ {
+ unselect_card();
+ return 0;
+ }
+
+ /* send start byte */
+ sd_raw_send_byte(0xfe);
+
+ /* write byte block */
+ uint8_t* cache = raw_block;
+ for(uint16_t i = 0; i < 512; ++i)
+ sd_raw_send_byte(*cache++);
+
+ /* write dummy crc16 */
+ sd_raw_send_byte(0xff);
+ sd_raw_send_byte(0xff);
+
+ /* wait while card is busy */
+ while(sd_raw_rec_byte() != 0xff);
+ sd_raw_rec_byte();
+
+ /* deaddress card */
+ unselect_card();
+
+ buffer += write_length;
+ offset += write_length;
+ length -= write_length;
+
+#if SD_RAW_WRITE_BUFFERING
+ raw_block_written = 1;
+#endif
+ }
+
+ return 1;
+}
+#endif
+
+#if DOXYGEN || SD_RAW_WRITE_SUPPORT
+/**
+ * \ingroup sd_raw
+ * Writes a continuous data stream obtained from a callback function.
+ *
+ * This function starts writing at the specified offset. To obtain the
+ * next bytes to write, it calls the callback function. The callback fills the
+ * provided data buffer and returns the number of bytes it has put into the buffer.
+ *
+ * By returning zero, the callback may stop writing.
+ *
+ * \param[in] offset Offset where to start writing.
+ * \param[in] buffer Pointer to a buffer which is used for the callback function.
+ * \param[in] length Number of bytes to write in total. May be zero for endless writes.
+ * \param[in] callback The function used to obtain the bytes to write.
+ * \param[in] p An opaque pointer directly passed to the callback function.
+ * \returns 0 on failure, 1 on success
+ * \see sd_raw_read_interval, sd_raw_write, sd_raw_read
+ */
+uint8_t sd_raw_write_interval(offset_t offset, uint8_t* buffer, uintptr_t length, sd_raw_write_interval_handler_t callback, void* p)
+{
+#if SD_RAW_SAVE_RAM
+ #error "SD_RAW_WRITE_SUPPORT is not supported together with SD_RAW_SAVE_RAM"
+#endif
+
+ if(!buffer || !callback)
+ return 0;
+
+ uint8_t endless = (length == 0);
+ while(endless || length > 0)
+ {
+ uint16_t bytes_to_write = callback(buffer, offset, p);
+ if(!bytes_to_write)
+ break;
+ if(!endless && bytes_to_write > length)
+ return 0;
+
+ /* as writing is always buffered, we directly
+ * hand over the request to sd_raw_write()
+ */
+ if(!sd_raw_write(offset, buffer, bytes_to_write))
+ return 0;
+
+ offset += bytes_to_write;
+ length -= bytes_to_write;
+ }
+
+ return 1;
+}
+#endif
+
+#if DOXYGEN || SD_RAW_WRITE_SUPPORT
+/**
+ * \ingroup sd_raw
+ * Writes the write buffer's content to the card.
+ *
+ * \note When write buffering is enabled, you should
+ * call this function before disconnecting the
+ * card to ensure all remaining data has been
+ * written.
+ *
+ * \returns 0 on failure, 1 on success.
+ * \see sd_raw_write
+ */
+uint8_t sd_raw_sync()
+{
+#if SD_RAW_WRITE_BUFFERING
+ if(raw_block_written)
+ return 1;
+ if(!sd_raw_write(raw_block_address, raw_block, sizeof(raw_block)))
+ return 0;
+ raw_block_written = 1;
+#endif
+ return 1;
+}
+#endif
+
+/**
+ * \ingroup sd_raw
+ * Reads informational data from the card.
+ *
+ * This function reads and returns the card's registers
+ * containing manufacturing and status information.
+ *
+ * \note: The information retrieved by this function is
+ * not required in any way to operate on the card,
+ * but it might be nice to display some of the data
+ * to the user.
+ *
+ * \param[in] info A pointer to the structure into which to save the information.
+ * \returns 0 on failure, 1 on success.
+ */
+uint8_t sd_raw_get_info(struct sd_raw_info* info)
+{
+ if(!info || !sd_raw_available())
+ return 0;
+
+ memset(info, 0, sizeof(*info));
+
+ select_card();
+
+ /* read cid register */
+ if(sd_raw_send_command(CMD_SEND_CID, 0))
+ {
+ unselect_card();
+ return 0;
+ }
+ while(sd_raw_rec_byte() != 0xfe);
+ for(uint8_t i = 0; i < 18; ++i)
+ {
+ uint8_t b = sd_raw_rec_byte();
+
+ switch(i)
+ {
+ case 0:
+ info->manufacturer = b;
+ break;
+ case 1:
+ case 2:
+ info->oem[i - 1] = b;
+ break;
+ case 3:
+ case 4:
+ case 5:
+ case 6:
+ case 7:
+ info->product[i - 3] = b;
+ break;
+ case 8:
+ info->revision = b;
+ break;
+ case 9:
+ case 10:
+ case 11:
+ case 12:
+ info->serial |= (uint32_t) b << ((12 - i) * 8);
+ break;
+ case 13:
+ info->manufacturing_year = b << 4;
+ break;
+ case 14:
+ info->manufacturing_year |= b >> 4;
+ info->manufacturing_month = b & 0x0f;
+ break;
+ }
+ }
+
+ /* read csd register */
+ uint8_t csd_read_bl_len = 0;
+ uint8_t csd_c_size_mult = 0;
+#if SD_RAW_SDHC
+ uint16_t csd_c_size = 0;
+#else
+ uint32_t csd_c_size = 0;
+#endif
+ uint8_t csd_structure = 0;
+ if(sd_raw_send_command(CMD_SEND_CSD, 0))
+ {
+ unselect_card();
+ return 0;
+ }
+ while(sd_raw_rec_byte() != 0xfe);
+ for(uint8_t i = 0; i < 18; ++i)
+ {
+ uint8_t b = sd_raw_rec_byte();
+
+ if(i == 0)
+ {
+ csd_structure = b >> 6;
+ (void)csd_structure;
+ }
+ else if(i == 14)
+ {
+ if(b & 0x40)
+ info->flag_copy = 1;
+ if(b & 0x20)
+ info->flag_write_protect = 1;
+ if(b & 0x10)
+ info->flag_write_protect_temp = 1;
+ info->format = (b & 0x0c) >> 2;
+ }
+ else
+ {
+#if SD_RAW_SDHC
+ if(csd_structure == 0x01)
+ {
+ switch(i)
+ {
+ case 7:
+ b &= 0x3f;
+ case 8:
+ case 9:
+ csd_c_size <<= 8;
+ csd_c_size |= b;
+ break;
+ }
+ if(i == 9)
+ {
+ ++csd_c_size;
+ info->capacity = (offset_t) csd_c_size * 512 * 1024;
+ }
+ }
+ else if(csd_structure == 0x00)
+#endif
+ {
+ switch(i)
+ {
+ case 5:
+ csd_read_bl_len = b & 0x0f;
+ break;
+ case 6:
+ csd_c_size = b & 0x03;
+ csd_c_size <<= 8;
+ break;
+ case 7:
+ csd_c_size |= b;
+ csd_c_size <<= 2;
+ break;
+ case 8:
+ csd_c_size |= b >> 6;
+ ++csd_c_size;
+ break;
+ case 9:
+ csd_c_size_mult = b & 0x03;
+ csd_c_size_mult <<= 1;
+ break;
+ case 10:
+ csd_c_size_mult |= b >> 7;
+
+ info->capacity = (uint32_t) csd_c_size << (csd_c_size_mult + csd_read_bl_len + 2);
+ break;
+ }
+ }
+ }
+ }
+
+ unselect_card();
+
+ return 1;
+}
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SD_RAW_H
+#define SD_RAW_H
+
+#include <stdint.h>
+#include "sd_raw_config.h"
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup sd_raw
+ *
+ * @{
+ */
+/**
+ * \file
+ * MMC/SD/SDHC raw access header (license: GPLv2 or LGPLv2.1)
+ *
+ * \author Roland Riegel
+ */
+
+/**
+ * The card's layout is harddisk-like, which means it contains
+ * a master boot record with a partition table.
+ */
+#define SD_RAW_FORMAT_HARDDISK 0
+/**
+ * The card contains a single filesystem and no partition table.
+ */
+#define SD_RAW_FORMAT_SUPERFLOPPY 1
+/**
+ * The card's layout follows the Universal File Format.
+ */
+#define SD_RAW_FORMAT_UNIVERSAL 2
+/**
+ * The card's layout is unknown.
+ */
+#define SD_RAW_FORMAT_UNKNOWN 3
+
+/**
+ * This struct is used by sd_raw_get_info() to return
+ * manufacturing and status information of the card.
+ */
+struct sd_raw_info
+{
+ /**
+ * A manufacturer code globally assigned by the SD card organization.
+ */
+ uint8_t manufacturer;
+ /**
+ * A string describing the card's OEM or content, globally assigned by the SD card organization.
+ */
+ uint8_t oem[3];
+ /**
+ * A product name.
+ */
+ uint8_t product[6];
+ /**
+ * The card's revision, coded in packed BCD.
+ *
+ * For example, the revision value \c 0x32 means "3.2".
+ */
+ uint8_t revision;
+ /**
+ * A serial number assigned by the manufacturer.
+ */
+ uint32_t serial;
+ /**
+ * The year of manufacturing.
+ *
+ * A value of zero means year 2000.
+ */
+ uint8_t manufacturing_year;
+ /**
+ * The month of manufacturing.
+ */
+ uint8_t manufacturing_month;
+ /**
+ * The card's total capacity in bytes.
+ */
+ offset_t capacity;
+ /**
+ * Defines wether the card's content is original or copied.
+ *
+ * A value of \c 0 means original, \c 1 means copied.
+ */
+ uint8_t flag_copy;
+ /**
+ * Defines wether the card's content is write-protected.
+ *
+ * \note This is an internal flag and does not represent the
+ * state of the card's mechanical write-protect switch.
+ */
+ uint8_t flag_write_protect;
+ /**
+ * Defines wether the card's content is temporarily write-protected.
+ *
+ * \note This is an internal flag and does not represent the
+ * state of the card's mechanical write-protect switch.
+ */
+ uint8_t flag_write_protect_temp;
+ /**
+ * The card's data layout.
+ *
+ * See the \c SD_RAW_FORMAT_* constants for details.
+ *
+ * \note This value is not guaranteed to match reality.
+ */
+ uint8_t format;
+};
+
+typedef uint8_t (*sd_raw_read_interval_handler_t)(uint8_t* buffer, offset_t offset, void* p);
+typedef uintptr_t (*sd_raw_write_interval_handler_t)(uint8_t* buffer, offset_t offset, void* p);
+
+uint8_t sd_raw_init(void);
+uint8_t sd_raw_available(void);
+uint8_t sd_raw_locked(void);
+
+uint8_t sd_raw_read(offset_t offset, uint8_t* buffer, uintptr_t length);
+uint8_t sd_raw_read_interval(offset_t offset, uint8_t* buffer, uintptr_t interval, uintptr_t length, sd_raw_read_interval_handler_t callback, void* p);
+uint8_t sd_raw_write(offset_t offset, const uint8_t* buffer, uintptr_t length);
+uint8_t sd_raw_write_interval(offset_t offset, uint8_t* buffer, uintptr_t length, sd_raw_write_interval_handler_t callback, void* p);
+uint8_t sd_raw_sync(void);
+
+uint8_t sd_raw_get_info(struct sd_raw_info* info);
+
+/**
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+
+/*
+ * Copyright (c) 2006-2012 by Roland Riegel <feedback@roland-riegel.de>
+ *
+ * This file is free software; you can redistribute it and/or modify
+ * it under the terms of either the GNU General Public License version 2
+ * or the GNU Lesser General Public License version 2.1, both as
+ * published by the Free Software Foundation.
+ */
+
+#ifndef SD_RAW_CONFIG_H
+#define SD_RAW_CONFIG_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/**
+ * \addtogroup sd_raw
+ *
+ * @{
+ */
+/**
+ * \file
+ * MMC/SD support configuration (license: GPLv2 or LGPLv2.1)
+ */
+
+/**
+ * \ingroup sd_raw_config
+ * Controls MMC/SD write support.
+ *
+ * Set to 1 to enable MMC/SD write support, set to 0 to disable it.
+ */
+#define SD_RAW_WRITE_SUPPORT 1
+
+/**
+ * \ingroup sd_raw_config
+ * Controls MMC/SD write buffering.
+ *
+ * Set to 1 to buffer write accesses, set to 0 to disable it.
+ *
+ * \note This option has no effect when SD_RAW_WRITE_SUPPORT is 0.
+ */
+#define SD_RAW_WRITE_BUFFERING 1
+
+/**
+ * \ingroup sd_raw_config
+ * Controls MMC/SD access buffering.
+ *
+ * Set to 1 to save static RAM, but be aware that you will
+ * lose performance.
+ *
+ * \note When SD_RAW_WRITE_SUPPORT is 1, SD_RAW_SAVE_RAM will
+ * be reset to 0.
+ */
+#define SD_RAW_SAVE_RAM 1
+
+/**
+ * \ingroup sd_raw_config
+ * Controls support for SDHC cards.
+ *
+ * Set to 1 to support so-called SDHC memory cards, i.e. SD
+ * cards with more than 2 gigabytes of memory.
+ */
+#define SD_RAW_SDHC 1
+
+/**
+ * @}
+ */
+
+#define zer0
+#ifdef zer0
+ #define configure_pin_mosi() DDRB |= (1 << DDB5)
+ #define configure_pin_sck() DDRB |= (1 << DDB7)
+ #define configure_pin_ss() DDRC |= (1 << DDC6)
+ #define configure_pin_miso() DDRB &= ~(1 << DDB6)
+
+#define select_card() PORTC &= ~(1 << PORTC6)
+#define unselect_card() PORTC |= (1 << PORTC6)
+
+#else
+/* defines for customisation of sd/mmc port access */
+#if defined(__AVR_ATmega8__) || \
+ defined(__AVR_ATmega48__) || \
+ defined(__AVR_ATmega48P__) || \
+ defined(__AVR_ATmega88__) || \
+ defined(__AVR_ATmega88P__) || \
+ defined(__AVR_ATmega168__) || \
+ defined(__AVR_ATmega168P__) || \
+ defined(__AVR_ATmega328P__)
+ #define configure_pin_mosi() DDRB |= (1 << DDB3)
+ #define configure_pin_sck() DDRB |= (1 << DDB5)
+ #define configure_pin_ss() DDRB |= (1 << DDB2)
+ #define configure_pin_miso() DDRB &= ~(1 << DDB4)
+
+ #define select_card() PORTB &= ~(1 << PORTB2)
+ #define unselect_card() PORTB |= (1 << PORTB2)
+#elif defined(__AVR_ATmega16__) || \
+ defined(__AVR_ATmega32__)
+ #define configure_pin_mosi() DDRB |= (1 << DDB5)
+ #define configure_pin_sck() DDRB |= (1 << DDB7)
+ #define configure_pin_ss() DDRB |= (1 << DDB4)
+ #define configure_pin_miso() DDRB &= ~(1 << DDB6)
+
+ #define select_card() PORTB &= ~(1 << PORTB4)
+ #define unselect_card() PORTB |= (1 << PORTB4)
+#elif defined(__AVR_ATmega64__) || \
+ defined(__AVR_ATmega128__) || \
+ defined(__AVR_ATmega169__)
+ #define configure_pin_mosi() DDRB |= (1 << DDB2)
+ #define configure_pin_sck() DDRB |= (1 << DDB1)
+ #define configure_pin_ss() DDRB |= (1 << DDB0)
+ #define configure_pin_miso() DDRB &= ~(1 << DDB3)
+
+ #define select_card() PORTB &= ~(1 << PORTB0)
+ #define unselect_card() PORTB |= (1 << PORTB0)
+#else
+ #error "no sd/mmc pin mapping available!"
+#endif
+#endif
+
+#define configure_pin_available() do {} while (0) // DDRC &= ~(1 << DDC4)
+#define configure_pin_locked() do {} while (0) // DDRC &= ~(1 << DDC5)
+
+#define get_pin_available() (0) // (PINC & (1 << PINC4))
+#define get_pin_locked() (1) // (PINC & (1 << PINC5))
+
+#if SD_RAW_SDHC
+ typedef uint64_t offset_t;
+#else
+ typedef uint32_t offset_t;
+#endif
+
+/* configuration checks */
+#if SD_RAW_WRITE_SUPPORT
+#undef SD_RAW_SAVE_RAM
+#define SD_RAW_SAVE_RAM 0
+#else
+#undef SD_RAW_WRITE_BUFFERING
+#define SD_RAW_WRITE_BUFFERING 0
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2006)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: timer_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $
+ *
+ */
+
+#define TIMER0_ENABLED
+
+/* #define TIMER1_ENABLED */
+/* #define TIMER1A_ENABLED */
+/* #define TIMER1B_ENABLED */
+/* #define TIMER1C_ENABLED */
+
+/* #define TIMER2_ENABLED */
+
+/* #define TIMER3_ENABLED */
+/* #define TIMER3A_ENABLED */
+/* #define TIMER3B_ENABLED */
+/* #define TIMER3C_ENABLED */
+
+#define TIMER0_PRESCALER_DIV 8
--- /dev/null
+#! /usr/bin/env python
+import os
+
+
+
+filename = os.environ.get('PYTHONSTARTUP')
+if filename and os.path.isfile(filename):
+ execfile(filename)
+
+from miasm2.core.cpu import parse_ast
+from miasm2.arch.x86_arch import mn_x86, base_expr, variable
+from miasm2.core.bin_stream import bin_stream
+from miasm2.core import parse_asm
+from elfesteem import *
+from pdb import pm
+from miasm2.core import asmbloc
+import struct
+from miasm2.expression.expression import *
+
+
+def my__sub__(self, a):
+ return ExprOp('-', self, a)
+Expr.__sub__ = my__sub__
+
+
+def my_parse_op(t):
+ print "OP", t
+ v = t[0]
+ return (ExprOp, v)
+
+
+
+reg_and_id = {}
+
+def my_ast_int2expr(a):
+ return ExprInt32(a)
+def my_ast_id2expr(t):
+ #print "XXX", t
+ t = 'f_'+t
+ if not t in reg_and_id:
+ i = ExprId(t)
+ reg_and_id[t] = i
+ return reg_and_id.get(t, ExprId(t, size=32))
+
+my_var_parser = parse_ast(my_ast_id2expr, my_ast_int2expr)
+base_expr.setParseAction(my_var_parser)
+
+all_bloc, symbol_pool = parse_asm.parse_txt(mn_x86, 32, '''
+main:
+ PUSH q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3
+''')
+
+
+
+a = all_bloc[0][0].lines[0].args[0]
+
+
+
+
+def ExprOp_to_Math(self):
+ print self.op, self.args
+ ops2 = {'+':'f32_add',
+ '*':'f32_mul',
+ '-':'f32_sub',
+ }
+ out = [x.Expr_to_Math() for x in self.args]
+ if len(self.args) == 1:
+ return "f32_neg(%s)"%(out[0])
+ elif self.op in ops2:
+ last = out.pop()
+ print out
+ while out:
+ o = out.pop()
+ last = '%s(%s, %s)'%(ops2[self.op],
+ o, last)
+ return last
+ fds
+def ExprId_to_Math(self):
+ return self.name
+
+
+def ExprInt_to_Math(self):
+ return "f32_from_double(%s)"%int(self.arg)
+ExprOp.Expr_to_Math = ExprOp_to_Math
+ExprId.Expr_to_Math = ExprId_to_Math
+ExprInt.Expr_to_Math = ExprInt_to_Math
+
+xx = a.Expr_to_Math()
+print xx
+
--- /dev/null
+/*\r
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
+ *\r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation; either version 2 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ *\r
+ * Revision : $Id: uart_config.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $\r
+ *\r
+ */\r
+\r
+/* Droids-corp 2004 - Zer0\r
+ * config for uart module\r
+ */\r
+\r
+#ifndef UART_CONFIG_H\r
+#define UART_CONFIG_H\r
+\r
+/*\r
+ * UART0 definitions\r
+ */\r
+\r
+/* compile uart0 fonctions, undefine it to pass compilation */\r
+#define UART0_COMPILE\r
+\r
+/* enable uart0 if == 1, disable if == 0 */\r
+#define UART0_ENABLED 1\r
+\r
+/* enable uart0 interrupts if == 1, disable if == 0 */\r
+#define UART0_INTERRUPT_ENABLED 1\r
+\r
+#define UART0_BAUDRATE 57600\r
+\r
+/*\r
+ * if you enable this, the maximum baudrate you can reach is\r
+ * higher, but the precision is lower.\r
+ */\r
+#define UART0_USE_DOUBLE_SPEED 1\r
+\r
+#define UART0_RX_FIFO_SIZE 64\r
+#define UART0_TX_FIFO_SIZE 127\r
+#define UART0_NBITS 8\r
+\r
+#define UART0_PARITY UART_PARTITY_NONE\r
+\r
+#define UART0_STOP_BIT UART_STOP_BITS_1\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+/*\r
+ * UART1 definitions\r
+ */\r
+\r
+/* compile uart1 fonctions, undefine it to pass compilation */\r
+#define UART1_COMPILE\r
+\r
+/* enable uart1 if == 1, disable if == 0 */\r
+#define UART1_ENABLED 1\r
+\r
+/* enable uart1 interrupts if == 1, disable if == 0 */\r
+#define UART1_INTERRUPT_ENABLED 1\r
+\r
+#define UART1_BAUDRATE 57600\r
+\r
+/*\r
+ * if you enable this, the maximum baudrate you can reach is\r
+ * higher, but the precision is lower.\r
+ */\r
+#define UART1_USE_DOUBLE_SPEED 1\r
+\r
+#define UART1_RX_FIFO_SIZE 64\r
+#define UART1_TX_FIFO_SIZE 127\r
+#define UART1_NBITS 8\r
+\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+#endif\r
+\r
--- /dev/null
+TARGET = main
+
+AVERSIVE_DIR ?= ../..
+
+# List C source files here. (C dependencies are automatically generated.)
+SRC = $(TARGET).c
+SRC += xbee_user.c
+SRC += spi_servo.c
+SRC += commands.c
+SRC += commands_gen.c
+SRC += rc_proto.c
+SRC += callout.c
+SRC += parse_neighbor.c
+SRC += parse_atcmd.c
+SRC += parse_monitor.c
+SRC += cmdline.c
+SRC += beep.c
+SRC += eeprom_config.c
+SRC += i2c_protocol.c
+
+CFLAGS += -W -Wall -Werror
+
+########################################
+
+-include .aversive_conf
+include $(AVERSIVE_DIR)/mk/aversive_project.mk
--- /dev/null
+/* Default linker script, for normal executables */
+OUTPUT_FORMAT("elf32-avr","elf32-avr","elf32-avr")
+OUTPUT_ARCH(avr:6)
+MEMORY
+{
+ text (rx) : ORIGIN = 0, LENGTH = 1024K
+ data (rw!x) : ORIGIN = 0x800200, LENGTH = 0xfe00
+ eeprom (rw!x) : ORIGIN = 0x810000, LENGTH = 64K
+ fuse (rw!x) : ORIGIN = 0x820000, LENGTH = 1K
+ lock (rw!x) : ORIGIN = 0x830000, LENGTH = 1K
+ signature (rw!x) : ORIGIN = 0x840000, LENGTH = 1K
+}
+SECTIONS
+{
+ /* Read-only sections, merged into text segment: */
+ .hash : { *(.hash) }
+ .dynsym : { *(.dynsym) }
+ .dynstr : { *(.dynstr) }
+ .gnu.version : { *(.gnu.version) }
+ .gnu.version_d : { *(.gnu.version_d) }
+ .gnu.version_r : { *(.gnu.version_r) }
+ .rel.init : { *(.rel.init) }
+ .rela.init : { *(.rela.init) }
+ .rel.text :
+ {
+ *(.rel.text)
+ *(.rel.text.*)
+ *(.rel.gnu.linkonce.t*)
+ }
+ .rela.text :
+ {
+ *(.rela.text)
+ *(.rela.text.*)
+ *(.rela.gnu.linkonce.t*)
+ }
+ .rel.fini : { *(.rel.fini) }
+ .rela.fini : { *(.rela.fini) }
+ .rel.rodata :
+ {
+ *(.rel.rodata)
+ *(.rel.rodata.*)
+ *(.rel.gnu.linkonce.r*)
+ }
+ .rela.rodata :
+ {
+ *(.rela.rodata)
+ *(.rela.rodata.*)
+ *(.rela.gnu.linkonce.r*)
+ }
+ .rel.data :
+ {
+ *(.rel.data)
+ *(.rel.data.*)
+ *(.rel.gnu.linkonce.d*)
+ }
+ .rela.data :
+ {
+ *(.rela.data)
+ *(.rela.data.*)
+ *(.rela.gnu.linkonce.d*)
+ }
+ .rel.ctors : { *(.rel.ctors) }
+ .rela.ctors : { *(.rela.ctors) }
+ .rel.dtors : { *(.rel.dtors) }
+ .rela.dtors : { *(.rela.dtors) }
+ .rel.got : { *(.rel.got) }
+ .rela.got : { *(.rela.got) }
+ .rel.bss : { *(.rel.bss) }
+ .rela.bss : { *(.rela.bss) }
+ .rel.plt : { *(.rel.plt) }
+ .rela.plt : { *(.rela.plt) }
+ /* Internal text space or external memory. */
+ .text :
+ {
+ *(.vectors)
+ KEEP(*(.vectors))
+ . = 256 + ALIGN(256); /* placeholder for misc microb infos */
+ /* For data that needs to reside in the lower 64k of progmem. */
+ *(.progmem.gcc*)
+ *(.progmem*)
+ . = ALIGN(2048);
+ __trampolines_start = . ;
+ /* The jump trampolines for the 16-bit limited relocs will reside here. */
+ *(.trampolines)
+ *(.trampolines*)
+ __trampolines_end = . ;
+ /* For future tablejump instruction arrays for 3 byte pc devices.
+ We don't relax jump/call instructions within these sections. */
+ *(.jumptables)
+ *(.jumptables*)
+ /* For code that needs to reside in the lower 128k progmem. */
+ *(.lowtext)
+ *(.lowtext*)
+ __ctors_start = . ;
+ *(.ctors)
+ __ctors_end = . ;
+ __dtors_start = . ;
+ *(.dtors)
+ __dtors_end = . ;
+ KEEP(SORT(*)(.ctors))
+ KEEP(SORT(*)(.dtors))
+ /* From this point on, we don't bother about wether the insns are
+ below or above the 16 bits boundary. */
+ *(.init0) /* Start here after reset. */
+ KEEP (*(.init0))
+ *(.init1)
+ KEEP (*(.init1))
+ *(.init2) /* Clear __zero_reg__, set up stack pointer. */
+ KEEP (*(.init2))
+ *(.init3)
+ KEEP (*(.init3))
+ *(.init4) /* Initialize data and BSS. */
+ KEEP (*(.init4))
+ *(.init5)
+ KEEP (*(.init5))
+ *(.init6) /* C++ constructors. */
+ KEEP (*(.init6))
+ *(.init7)
+ KEEP (*(.init7))
+ *(.init8)
+ KEEP (*(.init8))
+ *(.init9) /* Call main(). */
+ KEEP (*(.init9))
+ *(.text.*) /* trucs de gcc ? */
+ . = ALIGN(2048);
+ /* some libc stuff */
+ strc*(.text)
+ mem*(.text)
+ printf*(.text)
+ vfprintf*(.text)
+ sprintf*(.text)
+ snprintf*(.text)
+ malloc*(.text)
+ free*(.text)
+ fdevopen*(.text)
+ fputc*(.text)
+ . = ALIGN(2048);
+ uart*(.text)
+ parse*(.text)
+ rdline*(.text)
+ vt100*(.text)
+ scheduler*(.text)
+ i2c*(.text)
+ spi*(.text)
+ xbee*(.text)
+ commands*(.text)
+ . = ALIGN(2048);
+ *(.text)
+ . = ALIGN(2);
+ *(.fini9) /* _exit() starts here. */
+ KEEP (*(.fini9))
+ *(.fini8)
+ KEEP (*(.fini8))
+ *(.fini7)
+ KEEP (*(.fini7))
+ *(.fini6) /* C++ destructors. */
+ KEEP (*(.fini6))
+ *(.fini5)
+ KEEP (*(.fini5))
+ *(.fini4)
+ KEEP (*(.fini4))
+ *(.fini3)
+ KEEP (*(.fini3))
+ *(.fini2)
+ KEEP (*(.fini2))
+ *(.fini1)
+ KEEP (*(.fini1))
+ *(.fini0) /* Infinite loop after program termination. */
+ KEEP (*(.fini0))
+ _etext = . ;
+ } > text
+ .data : AT (ADDR (.text) + SIZEOF (.text))
+ {
+ PROVIDE (__data_start = .) ;
+ *(.data)
+ *(.data*)
+ *(.rodata) /* We need to include .rodata here if gcc is used */
+ *(.rodata*) /* with -fdata-sections. */
+ *(.gnu.linkonce.d*)
+ . = ALIGN(2);
+ _edata = . ;
+ PROVIDE (__data_end = .) ;
+ } > data
+ .bss SIZEOF(.data) + ADDR(.data) :
+ {
+ PROVIDE (__bss_start = .) ;
+ *(.bss)
+ *(.bss*)
+ *(COMMON)
+ PROVIDE (__bss_end = .) ;
+ } > data
+ __data_load_start = LOADADDR(.data);
+ __data_load_end = __data_load_start + SIZEOF(.data);
+ /* Global data not cleared after reset. */
+ .noinit SIZEOF(.bss) + ADDR(.bss) :
+ {
+ PROVIDE (__noinit_start = .) ;
+ *(.noinit*)
+ PROVIDE (__noinit_end = .) ;
+ _end = . ;
+ PROVIDE (__heap_start = .) ;
+ } > data
+ .eeprom :
+ {
+ *(.eeprom*)
+ __eeprom_end = . ;
+ } > eeprom
+ .fuse :
+ {
+ KEEP(*(.fuse))
+ KEEP(*(.lfuse))
+ KEEP(*(.hfuse))
+ KEEP(*(.efuse))
+ } > fuse
+ .lock :
+ {
+ KEEP(*(.lock*))
+ } > lock
+ .signature :
+ {
+ KEEP(*(.signature*))
+ } > signature
+ /* Stabs debugging sections. */
+ .stab 0 : { *(.stab) }
+ .stabstr 0 : { *(.stabstr) }
+ .stab.excl 0 : { *(.stab.excl) }
+ .stab.exclstr 0 : { *(.stab.exclstr) }
+ .stab.index 0 : { *(.stab.index) }
+ .stab.indexstr 0 : { *(.stab.indexstr) }
+ .comment 0 : { *(.comment) }
+ /* DWARF debug sections.
+ Symbols in the DWARF debugging sections are relative to the beginning
+ of the section so we begin them at 0. */
+ /* DWARF 1 */
+ .debug 0 : { *(.debug) }
+ .line 0 : { *(.line) }
+ /* GNU DWARF 1 extensions */
+ .debug_srcinfo 0 : { *(.debug_srcinfo) }
+ .debug_sfnames 0 : { *(.debug_sfnames) }
+ /* DWARF 1.1 and DWARF 2 */
+ .debug_aranges 0 : { *(.debug_aranges) }
+ .debug_pubnames 0 : { *(.debug_pubnames) }
+ /* DWARF 2 */
+ .debug_info 0 : { *(.debug_info) *(.gnu.linkonce.wi.*) }
+ .debug_abbrev 0 : { *(.debug_abbrev) }
+ .debug_line 0 : { *(.debug_line) }
+ .debug_frame 0 : { *(.debug_frame) }
+ .debug_str 0 : { *(.debug_str) }
+ .debug_loc 0 : { *(.debug_loc) }
+ .debug_macinfo 0 : { *(.debug_macinfo) }
+}
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdint.h>
+#include <stdio.h>
+
+#include <aversive.h>
+#include <aversive/wait.h>
+
+#include <cirbuf.h>
+
+#include "main.h"
+
+/* 100 ms */
+#define BEEP_PERIOD_MS 100
+
+static struct cirbuf beep_fifo;
+static char beep_fifo_buf[16];
+volatile uint8_t beep_mask = 1; /* init beep */
+
+static struct callout beep_timer;
+
+union beep_t {
+ uint8_t u08;
+ struct {
+ uint8_t tone:2;
+ uint8_t len:3;
+ uint8_t pause:3;
+ };
+};
+static volatile union beep_t current_beep;
+
+/* called by the scheduler */
+static void beep_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+ (void)arg;
+
+ beep_mask = 0;
+ if (current_beep.len == 0 && current_beep.pause == 0) {
+ if (CIRBUF_GET_LEN(&beep_fifo) == 0)
+ goto reschedule;
+
+ current_beep.u08 = cirbuf_get_head(&beep_fifo);
+ cirbuf_del_head(&beep_fifo);
+ }
+
+ if (current_beep.len > 0) {
+ current_beep.len --;
+ switch (current_beep.tone) {
+ case 0: beep_mask = 1; break;
+ case 1: beep_mask = 2; break;
+ case 2: beep_mask = 4; break;
+ case 3: beep_mask = 8; break;
+ default: break;
+ }
+ goto reschedule;
+ }
+
+ if (current_beep.pause > 0) {
+ current_beep.pause --;
+ }
+
+ reschedule:
+ callout_reschedule(cm, tim, BEEP_PERIOD_MS);
+}
+
+void beep(uint8_t tone, uint8_t len, uint8_t pause)
+{
+ uint8_t flags;
+ union beep_t b;
+
+ b.tone = tone;
+ b.len = len;
+ b.pause = pause;
+ IRQ_LOCK(flags);
+ cirbuf_add_tail(&beep_fifo, b.u08);
+ IRQ_UNLOCK(flags);
+}
+
+void beep_init(void)
+{
+ cirbuf_init(&beep_fifo, beep_fifo_buf, 0, sizeof(beep_fifo_buf));
+ callout_init(&beep_timer, beep_cb, NULL, BEEP_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &beep_timer, BEEP_PERIOD_MS);
+}
--- /dev/null
+/*
+ * Copyright (c) 2013, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+extern volatile uint8_t beep_mask;
+
+/* tone between 0 and 3, len between 0 and 7, pause between 0 and 7 */
+void beep(uint8_t tone, uint8_t len, uint8_t pause);
+void beep_init(void);
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.c,v 1.7 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/error.h>
+#include <aversive/queue.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <uart.h>
+
+#include "callout.h"
+#include "main.h"
+#include "cmdline.h"
+
+#define FLUSH_LOGS_MS 1000 /* every second */
+#define LOG_PER_SEC_MAX 10
+
+extern const parse_ctx_t PROGMEM main_ctx[];
+
+static struct callout flush_log_timer;
+static uint8_t log_count;
+
+int cmdline_dev_send(char c, FILE* f)
+{
+ (void)f;
+ uart_send(CMDLINE_UART, c);
+ return 0;
+}
+
+int cmdline_dev_recv(FILE* f)
+{
+ int16_t c;
+
+ (void)f;
+ c = uart_recv_nowait(CMDLINE_UART);
+ if (c < 0)
+ return _FDEV_EOF;
+
+ return c;
+}
+
+
+int xbee_dev_send(char c, FILE* f)
+{
+ (void)f;
+ uart_send(XBEE_UART, c);
+ return 0;
+}
+
+int xbee_dev_recv(FILE* f)
+{
+ int16_t c;
+
+ (void)f;
+ c = uart_recv_nowait(XBEE_UART);
+ if (c < 0)
+ return _FDEV_EOF;
+
+ return c;
+}
+
+void cmdline_valid_buffer(const char *buf, uint8_t size)
+{
+ int8_t ret;
+ PGM_P ctx = (PGM_P)main_ctx;
+
+ (void)size;
+ ret = parse(ctx, buf);
+ if (ret == PARSE_AMBIGUOUS)
+ printf_P(PSTR("Ambiguous command\r\n"));
+ else if (ret == PARSE_NOMATCH)
+ printf_P(PSTR("Command not found\r\n"));
+ else if (ret == PARSE_BAD_ARGS)
+ printf_P(PSTR("Bad arguments\r\n"));
+}
+
+static int8_t
+complete_buffer(const char *buf, char *dstbuf, uint8_t dstsize,
+ int16_t *state)
+{
+ PGM_P ctx = (PGM_P)main_ctx;
+ return complete(ctx, buf, state, dstbuf, dstsize);
+}
+
+
+void cmdline_write_char(char c)
+{
+ cmdline_dev_send(c, NULL);
+}
+
+
+/* sending "pop" on cmdline uart resets the robot */
+void emergency(char c)
+{
+ static uint8_t i = 0;
+
+ if ((i == 0 && c == 'p') ||
+ (i == 1 && c == 'o') ||
+ (i == 2 && c == 'p'))
+ i++;
+ else if ( !(i == 1 && c == 'p') )
+ i = 0;
+ if (i == 3) {
+ //bootloader();
+ reset();
+ }
+}
+
+/* log function, configured dynamically */
+void mylog(struct error * e, ...)
+{
+#ifndef HOST_VERSION
+ u16 stream_flags = stdout->flags;
+#endif
+ va_list ap;
+ uint8_t i, flags;
+ uint32_t ms;
+ uint8_t prio;
+
+ /* too many logs */
+ if (log_count >= LOG_PER_SEC_MAX)
+ return;
+
+ /* higher log value means lower criticity */
+ if (e->severity > ERROR_SEVERITY_ERROR) {
+ if (xbeeboard.log_level < e->severity)
+ return;
+
+ for (i=0; i<NB_LOGS+1; i++)
+ if (xbeeboard.logs[i] == e->err_num)
+ break;
+ if (i == NB_LOGS+1)
+ return;
+ }
+
+ /* get time */
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ IRQ_UNLOCK(flags);
+
+ /* prevent flush log to occur */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm,
+ LOW_PRIO);
+
+ /* display the log */
+ va_start(ap, e);
+ printf_P(PSTR("%d.%.3d: "), (int)(ms/1000UL), (int)(ms%1000UL));
+ vfprintf_P(stdout, e->text, ap);
+ printf_P(PSTR("\r\n"));
+ va_end(ap);
+
+#ifndef HOST_VERSION
+ stdout->flags = stream_flags;
+#endif
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+}
+
+static void flush_logs_cb(struct callout_mgr *cm, struct callout *tim,
+ void *arg)
+{
+ (void)cm;
+ (void)tim;
+ (void)arg;
+
+ if (log_count == LOG_PER_SEC_MAX)
+ printf_P("some logs were dropped\n");
+ callout_reschedule(&xbeeboard.intr_cm, &flush_log_timer,
+ FLUSH_LOGS_MS);
+}
+
+
+int cmdline_poll(void)
+{
+ const char *history, *buffer;
+ int8_t ret, same = 0;
+ int16_t c;
+
+ c = cmdline_dev_recv(NULL);
+ if (c < 0)
+ return -1;
+
+ ret = rdline_char_in(&xbeeboard.rdl, c);
+ if (ret == 1) {
+ buffer = rdline_get_buffer(&xbeeboard.rdl);
+ history = rdline_get_history_item(&xbeeboard.rdl, 0);
+ if (history) {
+ same = !memcmp(buffer, history, strlen(history)) &&
+ buffer[strlen(history)] == '\n';
+ }
+ else
+ same = 0;
+ if (strlen(buffer) > 1 && !same)
+ rdline_add_history(&xbeeboard.rdl, buffer);
+
+ if (xbeeboard.rdl.status != RDLINE_STOPPED)
+ rdline_newline(&xbeeboard.rdl, xbeeboard.prompt);
+ }
+
+ return 0;
+}
+
+void cmdline_init(void)
+{
+ /* init command line */
+ rdline_init(&xbeeboard.rdl, cmdline_write_char, cmdline_valid_buffer,
+ complete_buffer);
+ snprintf_P(xbeeboard.prompt, sizeof(xbeeboard.prompt),
+ PSTR("mainboard > "));
+
+ /* load a timer for flushing logs */
+ callout_init(&flush_log_timer, flush_logs_cb, NULL, LOW_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &flush_log_timer, FLUSH_LOGS_MS);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation
+ * Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: cmdline.h,v 1.4 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#ifndef _CMDLINE_H_
+#define _CMDLINE_H_
+
+#define CMDLINE_UART 1
+#define XBEE_UART 0
+
+void cmdline_init(void);
+
+/* uart rx callback for reset() */
+void emergency(char c);
+
+/* log function */
+void mylog(struct error * e, ...);
+
+/* poll cmdline */
+int cmdline_poll(void);
+
+int cmdline_dev_send(char c, FILE* f);
+int cmdline_dev_recv(FILE* f);
+void cmdline_write_char(char c);
+void cmdline_valid_buffer(const char *buf, uint8_t size);
+
+int xbee_dev_send(char c, FILE* f);
+int xbee_dev_recv(FILE* f);
+
+static inline uint8_t cmdline_keypressed(void)
+{
+ return (uart_recv_nowait(CMDLINE_UART) != -1);
+}
+
+static inline int16_t cmdline_getchar(void)
+{
+ return uart_recv_nowait(CMDLINE_UART);
+}
+
+static inline uint8_t cmdline_getchar_wait(void)
+{
+ return uart_recv(CMDLINE_UART);
+}
+
+#endif /* _CMDLINE_H_ */
--- /dev/null
+/*
+ * Copyright Droids Corporation (2011)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands.c,v 1.9 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <inttypes.h>
+#include <aversive.h>
+#include <aversive/pgmspace.h>
+#include <aversive/queue.h>
+#include <aversive/endian.h>
+#include <aversive/error.h>
+#include <aversive/wait.h>
+#include <parse.h>
+#include <rdline.h>
+#include <parse_string.h>
+#include <parse_num.h>
+#include <uart.h>
+#include <xbee.h>
+#include <callout.h>
+
+#include "parse_atcmd.h"
+#include "parse_neighbor.h"
+#include "parse_monitor.h"
+
+#include "spi_servo.h"
+#include "rc_proto.h"
+#include "xbee_user.h"
+#include "main.h"
+#include "cmdline.h"
+#include "beep.h"
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
+#include "eeprom_config.h"
+
+/* commands_gen.c */
+extern const parse_inst_t PROGMEM cmd_reset;
+extern const parse_inst_t PROGMEM cmd_bootloader;
+extern const parse_inst_t PROGMEM cmd_log;
+extern const parse_inst_t PROGMEM cmd_log_show;
+extern const parse_inst_t PROGMEM cmd_log_type;
+extern const parse_inst_t PROGMEM cmd_stack_space;
+extern const parse_inst_t PROGMEM cmd_callout;
+
+static int monitor_period_ms = 1000;
+static int monitor_running = 0;
+static int monitor_count = 0;
+static struct callout monitor_event;
+struct monitor_reg *monitor_current;
+
+static int range_period_ms = 1000;
+static int range_powermask = 0x1F;
+static uint8_t range_power = 0;
+static int range_running = 0;
+static uint64_t range_dstaddr = 0xFFFF; /* broadcast by default */
+static struct callout range_event;
+static int range_count = 100;
+static int range_cur_count = 0;
+
+static void monitor_cb(struct callout_mgr *cm,
+ struct callout *clt, void *dummy)
+{
+ (void)clt;
+ (void)dummy;
+
+ if (monitor_current == NULL)
+ monitor_current = LIST_FIRST(&xbee_monitor_list);
+
+ /* no rx_cb given: the user must check the monitored values in logs */
+ xbeeapp_send_atcmd(monitor_current->atcmd, NULL, 0, NULL, NULL);
+ monitor_current = LIST_NEXT(monitor_current, next);
+ callout_reschedule(cm, clt, monitor_period_ms / monitor_count);
+}
+
+static void range_cb(struct callout_mgr *cm,
+ struct callout *clt, void *dummy)
+{
+ struct rc_proto_power_probe power_probe;
+ struct xbee_msg msg;
+ uint8_t i, mask;
+
+ (void)clt;
+ (void)dummy;
+
+ range_cur_count--;
+
+ /* get new xmit power */
+ for (i = 1; i <= 8; i++) {
+ mask = 1 << ((range_power + i) & 0x7);
+ if (mask & range_powermask)
+ break;
+ }
+ range_power = ((range_power + i) & 0x7);
+
+ xbeeapp_send_atcmd("PL", &range_power, sizeof(range_power), NULL, NULL);
+
+ power_probe.type = RC_PROTO_POWER_PROBE;
+ power_probe.power_level = range_power;
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = &power_probe;
+ msg.iov[0].len = sizeof(power_probe);
+
+ xbeeapp_send_msg(range_dstaddr, &msg, NULL, NULL);
+
+ if (range_cur_count == 0) {
+ range_running = 0;
+ callout_stop(cm, clt);
+ return;
+ }
+
+ callout_reschedule(cm, clt, range_period_ms);
+}
+
+/* callback invoked when a xbee_send is done */
+static int8_t send_msg_cb(int8_t retcode, void *frame, unsigned len,
+ void *arg)
+{
+ struct xbee_xmit_status_hdr *recvframe = frame;
+ uint8_t *done = arg;
+
+ *done = 1;
+ if (retcode == XBEE_USER_RETCODE_TIMEOUT) {
+ printf_P(PSTR("timeout\r\n"));
+ return retcode;
+ }
+ if (retcode == XBEE_USER_RETCODE_BAD_FRAME ||
+ len != sizeof(*recvframe)) {
+ printf_P(PSTR("invalid frame\r\n"));
+ return XBEE_USER_RETCODE_BAD_FRAME;
+ }
+
+ printf_P(PSTR("ok\r\n"));
+ return XBEE_USER_RETCODE_OK;
+}
+
+/* callback invoked to dump the response to AT command */
+static int8_t dump_xbee_atresp_cb(int8_t retcode, void *frame, unsigned len,
+ void *arg)
+{
+ struct xbee_atresp_hdr *recvframe = frame;
+ char atcmd_str[3];
+ char buf[32];
+ uint8_t *done = arg;
+
+ *done = 1;
+ if (retcode == XBEE_USER_RETCODE_TIMEOUT) {
+ printf_P(PSTR("timeout\r\n"));
+ return retcode;
+ }
+ if (retcode == XBEE_USER_RETCODE_BAD_FRAME ||
+ len < sizeof(*recvframe)) {
+ printf_P(PSTR("invalid frame\r\n"));
+ return XBEE_USER_RETCODE_BAD_FRAME;
+ }
+
+ /* get AT command from frame */
+ memcpy(atcmd_str, &recvframe->cmd, 2);
+ atcmd_str[2] = '\0';
+
+ atresp_to_str(buf, sizeof(buf), frame, len);
+ len -= sizeof(*recvframe);
+ printf_P(PSTR("status ok, len=%d, %s\n"), len, buf);
+ return XBEE_USER_RETCODE_OK;
+}
+
+/* this structure is filled when cmd_help is parsed successfully */
+struct cmd_help_result {
+ fixed_string_t help;
+ struct xbee_atcmd *cmd;
+};
+
+/* function called when cmd_help is parsed successfully */
+static void cmd_help_parsed(void *parsed_result, void *data)
+{
+ struct cmd_help_result *res = parsed_result;
+ struct xbee_atcmd cmdcopy;
+ int type;
+
+ (void)data;
+
+ memcpy_P(&cmdcopy, res->cmd, sizeof(cmdcopy));
+ type = (cmdcopy.flags & (XBEE_ATCMD_F_READ | XBEE_ATCMD_F_WRITE));
+ switch (type) {
+ case XBEE_ATCMD_F_READ:
+ printf_P(PSTR("Read-only\r\n"));
+ break;
+ case XBEE_ATCMD_F_WRITE:
+ printf_P(PSTR("Write-only\r\n"));
+ break;
+ default:
+ printf_P(PSTR("Read-write\r\n"));
+ break;
+ }
+ if (cmdcopy.flags & XBEE_ATCMD_F_PARAM_NONE)
+ printf_P(PSTR("No argument\r\n"));
+ else if (cmdcopy.flags & XBEE_ATCMD_F_PARAM_U8)
+ printf_P(PSTR("Register is unsigned 8 bits\r\n"));
+ else if (cmdcopy.flags & XBEE_ATCMD_F_PARAM_U16)
+ printf_P(PSTR("Register is unsigned 16 bits\r\n"));
+ else if (cmdcopy.flags & XBEE_ATCMD_F_PARAM_U32)
+ printf_P(PSTR("Register is unsigned 32 bits\r\n"));
+ else if (cmdcopy.flags & XBEE_ATCMD_F_PARAM_S16)
+ printf_P(PSTR("Register is signed 16 bits\r\n"));
+ else if (cmdcopy.flags & XBEE_ATCMD_F_PARAM_STRING_20B)
+ printf_P(PSTR("Register is a 20 bytes string\r\n"));
+ else
+ printf_P(PSTR("Unknown argument\r\n"));
+
+ printf_P(PSTR("%S\r\n"), cmdcopy.help);
+}
+const char PROGMEM str_help_help[] = "help";
+
+const parse_token_string_t PROGMEM cmd_help_help =
+ TOKEN_STRING_INITIALIZER(struct cmd_help_result, help, str_help_help);
+
+const parse_token_atcmd_t PROGMEM cmd_help_atcmd =
+ TOKEN_ATCMD_INITIALIZER(struct cmd_help_result, cmd, &xbee_dev,
+ 0, 0);
+
+const char PROGMEM help_help[] = "Help a register using an AT command";
+const parse_inst_t PROGMEM cmd_help = {
+ .f = cmd_help_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_help,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_help_help,
+ (PGM_P)&cmd_help_atcmd,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_neigh_del_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+ struct xbee_neigh *neigh;
+};
+
+static void cmd_neigh_del_parsed(void *parsed_result,
+ void *data)
+{
+ struct cmd_neigh_del_result *res = parsed_result;
+
+ (void)data;
+ xbee_neigh_del(xbee_dev, res->neigh);
+}
+
+const char PROGMEM str_neigh_del_neigh[] = "neigh";
+const parse_token_string_t PROGMEM cmd_neigh_del_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_neigh_del_result, cmd,
+ str_neigh_del_neigh);
+const char PROGMEM str_neigh_del_del[] = "del";
+const parse_token_string_t PROGMEM cmd_neigh_del_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_neigh_del_result, action,
+ str_neigh_del_del);
+const parse_token_neighbor_t PROGMEM cmd_neigh_del_neigh =
+ TOKEN_NEIGHBOR_INITIALIZER(struct cmd_neigh_del_result, neigh,
+ &xbee_dev);
+
+const char PROGMEM help_neigh_del[] = "delete a neighbor";
+const parse_inst_t PROGMEM cmd_neigh_del = {
+ .f = cmd_neigh_del_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_neigh_del,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_neigh_del_cmd,
+ (PGM_P)&cmd_neigh_del_action,
+ (PGM_P)&cmd_neigh_del_neigh,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_neigh_add_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+ fixed_string_t name;
+ uint64_t addr;
+};
+
+static void cmd_neigh_add_parsed(void *parsed_result,
+ void *data)
+{
+ struct cmd_neigh_add_result *res = parsed_result;
+
+ (void)data;
+ if (xbee_neigh_add(xbee_dev, res->name, res->addr) == NULL)
+ printf_P(PSTR("name or addr already exist\r\n"));
+}
+
+const char PROGMEM str_neigh_add_neigh[] = "neigh";
+const parse_token_string_t PROGMEM cmd_neigh_add_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_neigh_add_result, cmd,
+ str_neigh_add_neigh);
+const char PROGMEM str_neigh_add_add[] = "add";
+const parse_token_string_t PROGMEM cmd_neigh_add_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_neigh_add_result, action,
+ str_neigh_add_add);
+const parse_token_string_t PROGMEM cmd_neigh_add_name =
+ TOKEN_STRING_INITIALIZER(struct cmd_neigh_add_result, name, NULL);
+const parse_token_num_t PROGMEM cmd_neigh_add_addr =
+ TOKEN_NUM_INITIALIZER(struct cmd_neigh_add_result, addr, UINT64);
+
+const char PROGMEM help_neigh_add[] = "add a neighbor";
+const parse_inst_t PROGMEM cmd_neigh_add = {
+ .f = cmd_neigh_add_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_neigh_add,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_neigh_add_cmd,
+ (PGM_P)&cmd_neigh_add_action,
+ (PGM_P)&cmd_neigh_add_name,
+ (PGM_P)&cmd_neigh_add_addr,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_neigh_list_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+};
+
+static void cmd_neigh_list_parsed(void *parsed_result,
+ void *data)
+{
+ struct xbee_neigh *neigh;
+
+ (void)parsed_result;
+ (void)data;
+ LIST_FOREACH(neigh, &xbee_dev->neigh_list, next) {
+ printf_P(PSTR(" %s: 0x%.8"PRIx32"%.8"PRIx32"\r\n"),
+ neigh->name,
+ (uint32_t)(neigh->addr >> 32ULL),
+ (uint32_t)(neigh->addr & 0xFFFFFFFF));
+ }
+}
+
+const char PROGMEM str_neigh_list_neigh[] = "neigh";
+const parse_token_string_t PROGMEM cmd_neigh_list_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_neigh_list_result, cmd,
+ str_neigh_list_neigh);
+const char PROGMEM str_neigh_list_list[] = "list";
+const parse_token_string_t PROGMEM cmd_neigh_list_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_neigh_list_result, action,
+ str_neigh_list_list);
+
+const char PROGMEM help_neigh_list[] = "list all knwon neighbors";
+const parse_inst_t PROGMEM cmd_neigh_list = {
+ .f = cmd_neigh_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_neigh_list,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_neigh_list_cmd,
+ (PGM_P)&cmd_neigh_list_action,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_read is parsed successfully */
+struct cmd_read_result {
+ fixed_string_t read;
+ struct xbee_atcmd *cmd;
+};
+
+/* function called when cmd_read is parsed successfully */
+static void cmd_read_parsed(void *parsed_result,
+ void *data)
+{
+ struct cmd_read_result *res = parsed_result;
+ struct xbee_atcmd copy;
+ char cmd[3];
+ volatile uint8_t done = 0;
+
+ (void)data;
+ memcpy_P(©, res->cmd, sizeof(copy));
+ memcpy_P(&cmd, copy.name, 2);
+ cmd[2] = '\0';
+ xbeeapp_send_atcmd(cmd, NULL, 0, dump_xbee_atresp_cb, (void *)&done);
+ while (done == 0);
+}
+
+const char PROGMEM str_read_read[] = "read";
+
+const parse_token_string_t PROGMEM cmd_read_read =
+ TOKEN_STRING_INITIALIZER(struct cmd_read_result, read,
+ str_read_read);
+
+const parse_token_atcmd_t PROGMEM cmd_read_atcmd =
+ TOKEN_ATCMD_INITIALIZER(struct cmd_read_result, cmd, &xbee_dev,
+ XBEE_ATCMD_F_READ, XBEE_ATCMD_F_READ);
+
+const char PROGMEM help_read[] = "Read a register using an AT command";
+const parse_inst_t PROGMEM cmd_read = {
+ .f = cmd_read_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_read,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_read_read,
+ (PGM_P)&cmd_read_atcmd,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+/* this structure is filled when cmd_write is parsed successfully */
+struct cmd_write_result {
+ fixed_string_t write;
+ struct xbee_atcmd *cmd;
+ union {
+ uint8_t u8;
+ uint16_t u16;
+ uint32_t u32;
+ };
+};
+
+/* function called when cmd_write is parsed successfully */
+static void cmd_write_parsed(void *parsed_result, void *data)
+{
+ struct cmd_write_result *res = parsed_result;
+ struct xbee_atcmd copy;
+ char cmd[3];
+ int len;
+ void *param;
+ volatile uint8_t done = 0;
+
+ (void)data;
+ memcpy_P(©, res->cmd, sizeof(copy));
+
+ if (copy.flags & XBEE_ATCMD_F_PARAM_NONE) {
+ len = 0;
+ param = NULL;
+ }
+ else if (copy.flags & XBEE_ATCMD_F_PARAM_U8) {
+ len = sizeof(res->u8);
+ param = &res->u8;
+ }
+ else if (copy.flags & XBEE_ATCMD_F_PARAM_U16) {
+ len = sizeof(res->u16);
+ res->u16 = htons(res->u16);
+ param = &res->u16;
+ }
+ else if (copy.flags & XBEE_ATCMD_F_PARAM_U32) {
+ len = sizeof(res->u32);
+ res->u32 = htonl(res->u32);
+ param = &res->u32;
+ }
+ else {
+ printf_P(PSTR("Unknown argument type\r\n"));
+ return;
+ }
+ memcpy_P(&cmd, copy.name, 2);
+ cmd[2] = '\0';
+ xbeeapp_send_atcmd(cmd, param, len, dump_xbee_atresp_cb, (void *)&done);
+ while (done == 0);
+}
+
+const char PROGMEM str_write_none[] = "write";
+
+const parse_token_string_t PROGMEM cmd_write_write =
+ TOKEN_STRING_INITIALIZER(struct cmd_write_result, write,
+ str_write_none);
+
+const parse_token_atcmd_t PROGMEM cmd_write_none_atcmd =
+ TOKEN_ATCMD_INITIALIZER(struct cmd_write_result, cmd,
+ &xbee_dev,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_NONE,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_NONE);
+
+const char PROGMEM help_write_none[] = "Send an AT command (no argument)";
+
+const parse_inst_t PROGMEM cmd_write_none = {
+ .f = cmd_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_write_none,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_write_write,
+ (PGM_P)&cmd_write_none_atcmd,
+ NULL,
+ },
+};
+
+const parse_token_atcmd_t PROGMEM cmd_write_u8_atcmd =
+ TOKEN_ATCMD_INITIALIZER(struct cmd_write_result, cmd,
+ &xbee_dev,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_U8,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_U8);
+
+const parse_token_num_t PROGMEM cmd_write_u8_u8 =
+ TOKEN_NUM_INITIALIZER(struct cmd_write_result, u8, UINT8);
+
+const char PROGMEM help_write_u8[] = "Write a 8 bits register using an AT command";
+
+const parse_inst_t PROGMEM cmd_write_u8 = {
+ .f = cmd_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_write_u8,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_write_write,
+ (PGM_P)&cmd_write_u8_atcmd,
+ (PGM_P)&cmd_write_u8_u8,
+ NULL,
+ },
+};
+
+const parse_token_atcmd_t PROGMEM cmd_write_u16_atcmd =
+ TOKEN_ATCMD_INITIALIZER(struct cmd_write_result, cmd,
+ &xbee_dev,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_U16,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_U16);
+
+const parse_token_num_t PROGMEM cmd_write_u16_u16 =
+ TOKEN_NUM_INITIALIZER(struct cmd_write_result, u16, UINT16);
+
+const char PROGMEM help_write_u16[] = "Write a 16 bits register using an AT command";
+
+const parse_inst_t PROGMEM cmd_write_u16 = {
+ .f = cmd_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_write_u16,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_write_write,
+ (PGM_P)&cmd_write_u16_atcmd,
+ (PGM_P)&cmd_write_u16_u16,
+ NULL,
+ },
+};
+
+const parse_token_atcmd_t PROGMEM cmd_write_u32_atcmd =
+ TOKEN_ATCMD_INITIALIZER(struct cmd_write_result, cmd,
+ &xbee_dev,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_U32,
+ XBEE_ATCMD_F_WRITE | XBEE_ATCMD_F_PARAM_U32);
+
+const parse_token_num_t PROGMEM cmd_write_u32_u32 =
+ TOKEN_NUM_INITIALIZER(struct cmd_write_result, u32, UINT32);
+
+const char PROGMEM help_write_u32[] = "Write a 32 bits register using an AT command";
+
+const parse_inst_t PROGMEM cmd_write_u32 = {
+ .f = cmd_write_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_write_u32,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_write_write,
+ (PGM_P)&cmd_write_u32_atcmd,
+ (PGM_P)&cmd_write_u32_u32,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+/* this structure is filled when cmd_sendmsg is parsed successfully */
+struct cmd_sendmsg_result {
+ fixed_string_t sendmsg;
+ uint64_t addr;
+ fixed_string_t data;
+};
+
+/* function called when cmd_sendmsg is parsed successfully */
+static void cmd_sendmsg_parsed(void *parsed_result, void *data)
+{
+ struct cmd_sendmsg_result *res = parsed_result;
+ struct xbee_msg msg;
+ volatile uint8_t done = 0;
+
+ (void)data;
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = res->data;
+ msg.iov[0].len = strlen(res->data);
+
+ xbeeapp_send_msg(res->addr, &msg, send_msg_cb, (void *)&done);
+ while (done == 0);
+}
+
+const char PROGMEM str_sendmsg[] = "sendmsg";
+
+const parse_token_string_t PROGMEM cmd_sendmsg_sendmsg =
+ TOKEN_STRING_INITIALIZER(struct cmd_sendmsg_result, sendmsg,
+ str_sendmsg);
+
+const parse_token_num_t PROGMEM cmd_sendmsg_addr =
+ TOKEN_NUM_INITIALIZER(struct cmd_sendmsg_result, addr, UINT64);
+
+const parse_token_string_t PROGMEM cmd_sendmsg_data =
+ TOKEN_STRING_INITIALIZER(struct cmd_sendmsg_result, data, NULL);
+
+const char PROGMEM help_sendmsg[] = "Send data to a node using its address";
+
+const parse_inst_t PROGMEM cmd_sendmsg = {
+ .f = cmd_sendmsg_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sendmsg,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_sendmsg_sendmsg,
+ (PGM_P)&cmd_sendmsg_addr,
+ (PGM_P)&cmd_sendmsg_data,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_sendmsg_name is parsed successfully */
+struct cmd_sendmsg_name_result {
+ fixed_string_t sendmsg_name;
+ struct xbee_neigh *neigh;
+ fixed_string_t data;
+};
+
+/* function called when cmd_sendmsg_name is parsed successfully */
+static void cmd_sendmsg_name_parsed(void *parsed_result, void *data)
+{
+ struct cmd_sendmsg_name_result *res = parsed_result;
+ struct xbee_msg msg;
+ volatile uint8_t done = 0;
+
+ (void)data;
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = res->data;
+ msg.iov[0].len = strlen(res->data);
+
+ xbeeapp_send_msg(res->neigh->addr, &msg, send_msg_cb, (void *)&done);
+ while (done == 0);
+}
+
+const parse_token_string_t PROGMEM cmd_sendmsg_name_sendmsg_name =
+ TOKEN_STRING_INITIALIZER(struct cmd_sendmsg_name_result, sendmsg_name,
+ str_sendmsg);
+
+const parse_token_neighbor_t PROGMEM cmd_sendmsg_name_neigh =
+ TOKEN_NEIGHBOR_INITIALIZER(struct cmd_sendmsg_name_result, neigh,
+ &xbee_dev);
+
+const parse_token_string_t PROGMEM cmd_sendmsg_name_data =
+ TOKEN_STRING_INITIALIZER(struct cmd_sendmsg_name_result, data, NULL);
+
+const char PROGMEM help_sendmsg_name[] = "Send data to a node using its name";
+
+const parse_inst_t PROGMEM cmd_sendmsg_name = {
+ .f = cmd_sendmsg_name_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_sendmsg_name,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_sendmsg_name_sendmsg_name,
+ (PGM_P)&cmd_sendmsg_name_neigh,
+ (PGM_P)&cmd_sendmsg_name_data,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+/* this structure is filled when cmd_range is parsed successfully */
+struct cmd_range_result {
+ fixed_string_t range;
+ fixed_string_t action;
+};
+
+/* function called when cmd_range is parsed successfully */
+static void cmd_range_parsed(void *parsed_result, void *data)
+{
+ struct cmd_range_result *res = parsed_result;
+
+ (void)data;
+ if (!strcmp_P(res->action, PSTR("show"))) {
+ printf_P(PSTR("range infos:\r\n"));
+ printf_P(PSTR(" range period %d\r\n"), range_period_ms);
+ printf_P(PSTR(" range count %d\r\n"), range_count);
+ printf_P(PSTR(" range powermask 0x%x\r\n"), range_powermask);
+ printf_P(PSTR(" range dstaddr 0x%.8"PRIx32"%.8"PRIx32"\r\n"),
+ (uint32_t)(range_dstaddr >> 32ULL),
+ (uint32_t)(range_dstaddr & 0xFFFFFFFF));
+
+ if (range_running)
+ printf_P(PSTR(" range test is running\r\n"));
+ else
+ printf_P(PSTR(" range test is not running\r\n"));
+ }
+ else if (!strcmp(res->action, "start")) {
+ if (range_running) {
+ printf_P(PSTR("already running\r\n"));
+ return;
+ }
+ range_cur_count = range_count;
+ callout_init(&range_event, range_cb, NULL, LOW_PRIO);
+ range_running = 1;
+ callout_schedule(&xbeeboard.intr_cm,
+ &range_event, 0); /* immediate */
+ }
+ else if (!strcmp(res->action, "end")) {
+ if (range_running == 0) {
+ printf_P(PSTR("not running\r\n"));
+ return;
+ }
+ callout_stop(&xbeeboard.intr_cm, &range_event);
+ range_running = 0;
+ }
+}
+
+const char PROGMEM str_range[] = "range";
+const char PROGMEM str_range_tokens[] = "show#start#end";
+
+const parse_token_string_t PROGMEM cmd_range_range =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_result, range,
+ str_range);
+const parse_token_string_t PROGMEM cmd_range_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_result, action,
+ str_range_tokens);
+
+const char PROGMEM help_range[] = "start/stop/show current rangeing";
+
+const parse_inst_t PROGMEM cmd_range = {
+ .f = cmd_range_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_range,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_range_range,
+ (PGM_P)&cmd_range_action,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_range_period is parsed successfully */
+struct cmd_range_period_result {
+ fixed_string_t range;
+ fixed_string_t action;
+ uint32_t period;
+};
+
+/* function called when cmd_range_period is parsed successfully */
+static void cmd_range_period_parsed(void *parsed_result, void *data)
+{
+ struct cmd_range_period_result *res = parsed_result;
+
+ (void)data;
+ if (res->period < 10) {
+ printf_P(PSTR("error, minimum period is 10 ms\r\n"));
+ return;
+ }
+
+ range_period_ms = res->period;
+}
+
+const char PROGMEM str_period[] = "period";
+
+const parse_token_string_t PROGMEM cmd_range_period_range_period =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_period_result, range,
+ str_range);
+const parse_token_string_t PROGMEM cmd_range_period_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_period_result, action,
+ str_period);
+const parse_token_num_t PROGMEM cmd_range_period_period =
+ TOKEN_NUM_INITIALIZER(struct cmd_range_period_result, period, UINT32);
+
+const char PROGMEM help_range_period[] = "set range test period";
+
+const parse_inst_t PROGMEM cmd_range_period = {
+ .f = cmd_range_period_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_range_period,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_range_period_range_period,
+ (PGM_P)&cmd_range_period_action,
+ (PGM_P)&cmd_range_period_period,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_range_count is parsed successfully */
+struct cmd_range_count_result {
+ fixed_string_t range;
+ fixed_string_t action;
+ uint32_t count;
+};
+
+/* function called when cmd_range_count is parsed successfully */
+static void cmd_range_count_parsed(void *parsed_result, void *data)
+{
+ struct cmd_range_count_result *res = parsed_result;
+
+ (void)data;
+ range_count = res->count;
+}
+
+const char PROGMEM str_count[] = "count";
+
+const parse_token_string_t PROGMEM cmd_range_count_range_count =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_count_result, range,
+ str_range);
+const parse_token_string_t PROGMEM cmd_range_count_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_count_result, action,
+ str_count);
+const parse_token_num_t PROGMEM cmd_range_count_count =
+ TOKEN_NUM_INITIALIZER(struct cmd_range_count_result, count, UINT32);
+
+
+const char PROGMEM help_range_count[] = "set range test count";
+
+const parse_inst_t PROGMEM cmd_range_count = {
+ .f = cmd_range_count_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_range_count,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_range_count_range_count,
+ (PGM_P)&cmd_range_count_action,
+ (PGM_P)&cmd_range_count_count,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_range_powermask is parsed successfully */
+struct cmd_range_powermask_result {
+ fixed_string_t range;
+ fixed_string_t action;
+ uint8_t powermask;
+};
+
+/* function called when cmd_range_powermask is parsed successfully */
+static void cmd_range_powermask_parsed(void *parsed_result, void *data)
+{
+ struct cmd_range_powermask_result *res = parsed_result;
+
+ (void)data;
+ range_powermask = res->powermask;
+}
+
+const char PROGMEM str_powermask[] = "powermask";
+
+const parse_token_string_t PROGMEM cmd_range_powermask_range_powermask =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_powermask_result, range,
+ str_range);
+const parse_token_string_t PROGMEM cmd_range_powermask_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_powermask_result, action,
+ str_powermask);
+const parse_token_num_t PROGMEM cmd_range_powermask_powermask =
+ TOKEN_NUM_INITIALIZER(struct cmd_range_powermask_result, powermask,
+ UINT8);
+
+
+const char PROGMEM help_range_powermask[] = "set range test powermask";
+
+const parse_inst_t PROGMEM cmd_range_powermask = {
+ .f = cmd_range_powermask_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_range_powermask,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_range_powermask_range_powermask,
+ (PGM_P)&cmd_range_powermask_action,
+ (PGM_P)&cmd_range_powermask_powermask,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_range_dstaddr is parsed successfully */
+struct cmd_range_dstaddr_result {
+ fixed_string_t range;
+ fixed_string_t action;
+ uint64_t dstaddr;
+};
+
+/* function called when cmd_range_dstaddr is parsed successfully */
+static void cmd_range_dstaddr_parsed(void *parsed_result, void *data)
+{
+ struct cmd_range_dstaddr_result *res = parsed_result;
+
+ (void)data;
+ range_dstaddr = res->dstaddr;
+}
+
+const char PROGMEM str_dstaddr[] = "dstaddr";
+
+const parse_token_string_t PROGMEM cmd_range_dstaddr_range_dstaddr =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_dstaddr_result, range,
+ str_range);
+const parse_token_string_t PROGMEM cmd_range_dstaddr_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_range_dstaddr_result, action,
+ str_dstaddr);
+const parse_token_num_t PROGMEM cmd_range_dstaddr_dstaddr =
+ TOKEN_NUM_INITIALIZER(struct cmd_range_dstaddr_result, dstaddr, UINT64);
+
+
+const char PROGMEM help_range_dstaddr[] = "set register rangeing dstaddr";
+
+const parse_inst_t PROGMEM cmd_range_dstaddr = {
+ .f = cmd_range_dstaddr_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_range_dstaddr,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_range_dstaddr_range_dstaddr,
+ (PGM_P)&cmd_range_dstaddr_action,
+ (PGM_P)&cmd_range_dstaddr_dstaddr,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+/* this structure is filled when cmd_monitor is parsed successfully */
+struct cmd_monitor_result {
+ fixed_string_t monitor;
+ fixed_string_t action;
+};
+
+/* function called when cmd_monitor is parsed successfully */
+static void cmd_monitor_parsed(void *parsed_result, void *data)
+{
+ struct cmd_monitor_result *res = parsed_result;
+ struct monitor_reg *m;
+
+ (void)data;
+ if (!strcmp_P(res->action, PSTR("show"))) {
+ printf_P(PSTR("monitor period is %d ms, %d regs in list\r\n"),
+ monitor_period_ms, monitor_count);
+ LIST_FOREACH(m, &xbee_monitor_list, next)
+ printf_P(PSTR(" %S\r\n"), m->desc);
+ }
+ else if (!strcmp_P(res->action, PSTR("start"))) {
+ if (monitor_running) {
+ printf_P(PSTR("already running\r\n"));
+ return;
+ }
+ if (monitor_count == 0) {
+ printf_P(PSTR("no regs to be monitored\r\n"));
+ return;
+ }
+ callout_init(&monitor_event, monitor_cb, NULL, 1);
+ monitor_running = 1;
+ monitor_current = LIST_FIRST(&xbee_monitor_list);
+ callout_schedule(&xbeeboard.intr_cm,
+ &monitor_event, 0); /* immediate */
+ printf_P(PSTR("monitor cb: %S %s\r\n"),
+ monitor_current->desc,
+ monitor_current->atcmd);
+
+ }
+ else if (!strcmp_P(res->action, PSTR("end"))) {
+ if (monitor_running == 0) {
+ printf_P(PSTR("not running\r\n"));
+ return;
+ }
+ callout_stop(&xbeeboard.intr_cm, &monitor_event);
+ monitor_running = 0;
+ }
+}
+
+const char PROGMEM str_monitor[] = "monitor";
+const char PROGMEM str_monitor_tokens[] = "show#start#end";
+
+const parse_token_string_t PROGMEM cmd_monitor_monitor =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_result, monitor,
+ str_monitor);
+const parse_token_string_t PROGMEM cmd_monitor_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_result, action,
+ str_monitor_tokens);
+
+const char PROGMEM help_monitor[] = "start/stop/show current monitoring";
+
+const parse_inst_t PROGMEM cmd_monitor = {
+ .f = cmd_monitor_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_monitor,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_monitor_monitor,
+ (PGM_P)&cmd_monitor_action,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_monitor_add is parsed successfully */
+struct cmd_monitor_add_result {
+ fixed_string_t monitor;
+ fixed_string_t action;
+ struct xbee_atcmd *cmd;
+};
+
+/* function called when cmd_monitor_add is parsed successfully */
+static void cmd_monitor_add_parsed(void *parsed_result, void *data)
+{
+ struct cmd_monitor_add_result *res = parsed_result;
+ struct monitor_reg *m;
+ struct xbee_atcmd copy;
+
+ (void)data;
+ memcpy_P(©, res->cmd, sizeof(copy));
+ LIST_FOREACH(m, &xbee_monitor_list, next) {
+ if (!strcmp_P(m->atcmd, copy.name))
+ break;
+ }
+
+ if (m != NULL) {
+ printf_P(PSTR("already exist\r\n"));
+ return;
+ }
+
+ m = malloc(sizeof(*m));
+ if (m == NULL) {
+ printf_P(PSTR("no mem\r\n"));
+ return;
+ }
+ m->desc = copy.desc;
+ strcpy_P(m->atcmd, copy.name);
+ LIST_INSERT_HEAD(&xbee_monitor_list, m, next);
+ monitor_count ++;
+}
+
+const char PROGMEM str_monitor_add[] = "add";
+
+const parse_token_string_t PROGMEM cmd_monitor_add_monitor_add =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_add_result, monitor,
+ str_monitor);
+const parse_token_string_t PROGMEM cmd_monitor_add_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_add_result, action,
+ str_monitor_add);
+const parse_token_atcmd_t PROGMEM cmd_monitor_add_atcmd =
+ TOKEN_ATCMD_INITIALIZER(struct cmd_monitor_add_result, cmd, &xbee_dev,
+ XBEE_ATCMD_F_READ, XBEE_ATCMD_F_READ);
+
+
+const char PROGMEM help_monitor_add[] = "add a register in monitor list";
+
+const parse_inst_t PROGMEM cmd_monitor_add = {
+ .f = cmd_monitor_add_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_monitor_add,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_monitor_add_monitor_add,
+ (PGM_P)&cmd_monitor_add_action,
+ (PGM_P)&cmd_monitor_add_atcmd,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_monitor_period is parsed successfully */
+struct cmd_monitor_period_result {
+ fixed_string_t monitor;
+ fixed_string_t action;
+ uint32_t period;
+};
+
+/* function called when cmd_monitor_period is parsed successfully */
+static void cmd_monitor_period_parsed(void *parsed_result, void *data)
+{
+ struct cmd_monitor_period_result *res = parsed_result;
+
+ (void)data;
+ if (res->period < 100) {
+ printf_P(PSTR("error, minimum period is 100 ms\r\n"));
+ return;
+ }
+
+ monitor_period_ms = res->period;
+}
+
+const char PROGMEM str_monitor_period[] = "period";
+
+const parse_token_string_t PROGMEM cmd_monitor_period_monitor_period =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_period_result, monitor,
+ str_monitor);
+const parse_token_string_t PROGMEM cmd_monitor_period_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_period_result, action,
+ str_monitor_period);
+const parse_token_num_t PROGMEM cmd_monitor_period_period =
+ TOKEN_NUM_INITIALIZER(struct cmd_monitor_period_result, period, UINT32);
+
+
+const char PROGMEM help_monitor_period[] = "set register monitoring period";
+
+const parse_inst_t PROGMEM cmd_monitor_period = {
+ .f = cmd_monitor_period_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_monitor_period,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_monitor_period_monitor_period,
+ (PGM_P)&cmd_monitor_period_action,
+ (PGM_P)&cmd_monitor_period_period,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_monitor_del is parsed successfully */
+struct cmd_monitor_del_result {
+ fixed_string_t monitor;
+ fixed_string_t action;
+ struct monitor_reg *m;
+};
+
+/* function called when cmd_monitor_del is parsed successfully */
+static void cmd_monitor_del_parsed(void *parsed_result, void *data)
+{
+ struct cmd_monitor_del_result *res = parsed_result;
+
+ (void)data;
+ monitor_current = LIST_NEXT(res->m, next);
+ LIST_REMOVE(res->m, next);
+ free(res->m);
+ monitor_count --;
+ if (monitor_count == 0) {
+ printf_P(PSTR("Disable monitoring, no more event\r\n"));
+ callout_stop(&xbeeboard.intr_cm, &monitor_event);
+ monitor_running = 0;
+ return;
+ }
+}
+
+const char PROGMEM str_monitor_del[] = "del";
+
+const parse_token_string_t PROGMEM cmd_monitor_del_monitor_del =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_del_result, monitor,
+ str_monitor);
+const parse_token_string_t PROGMEM cmd_monitor_del_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_monitor_del_result, action,
+ str_monitor_del);
+const parse_token_monitor_t PROGMEM cmd_monitor_del_atcmd =
+ TOKEN_MONITOR_INITIALIZER(struct cmd_monitor_del_result, m);
+
+
+const char PROGMEM help_monitor_del[] = "del a register in monitor list";
+
+const parse_inst_t PROGMEM cmd_monitor_del = {
+ .f = cmd_monitor_del_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_monitor_del,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_monitor_del_monitor_del,
+ (PGM_P)&cmd_monitor_del_action,
+ (PGM_P)&cmd_monitor_del_atcmd,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+/* this structure is filled when cmd_ping is parsed successfully */
+struct cmd_ping_result {
+ fixed_string_t ping;
+};
+
+/* function called when cmd_ping is parsed successfully */
+static void cmd_ping_parsed(void *parsed_result, void *data)
+{
+ volatile uint8_t done = 0;
+
+ (void)parsed_result;
+ (void)data;
+ xbeeapp_send_atcmd("VL", NULL, 0, dump_xbee_atresp_cb, (void *)&done);
+ while (done == 0);
+}
+
+const char PROGMEM str_ping[] = "ping";
+
+const parse_token_string_t PROGMEM cmd_ping_ping =
+ TOKEN_STRING_INITIALIZER(struct cmd_ping_result, ping,
+ str_ping);
+
+const char PROGMEM help_ping[] = "Send a ping to the xbee device";
+
+const parse_inst_t PROGMEM cmd_ping = {
+ .f = cmd_ping_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_ping,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_ping_ping,
+ NULL,
+ },
+};
+
+/* ************* */
+
+/* this structure is filled when cmd_raw is parsed successfully */
+struct cmd_raw_result {
+ fixed_string_t raw;
+};
+
+/* function called when cmd_raw is parsed successfully */
+static void cmd_raw_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+
+ if (range_running || monitor_running) {
+ printf_P(PSTR("stop running range or monitor first\r\n"));
+ return;
+ }
+ printf_P(PSTR("switched to raw mode, CTRL-D to exit\r\n"));
+ rdline_stop(&xbeeboard.rdl); /* don't display prompt when return */
+ xbee_raw = 1;
+}
+
+const char PROGMEM str_raw[] = "raw";
+
+const parse_token_string_t PROGMEM cmd_raw_raw =
+ TOKEN_STRING_INITIALIZER(struct cmd_raw_result, raw,
+ str_raw);
+
+const char PROGMEM help_raw[] = "Switch to raw mode";
+
+const parse_inst_t PROGMEM cmd_raw = {
+ .f = cmd_raw_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_raw,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_raw_raw,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_baudrate is parsed successfully */
+struct cmd_baudrate_result {
+ fixed_string_t arg0;
+ uint32_t arg1;
+};
+
+/* function called when cmd_baudrate is parsed successfully */
+static void cmd_baudrate_parsed(void * parsed_result, __attribute__((unused)) void *data)
+{
+ struct cmd_baudrate_result *res = parsed_result;
+ struct uart_config c;
+
+ uart_getconf(XBEE_UART, &c);
+ c.baudrate = res->arg1;
+ uart_setconf(XBEE_UART, &c);
+}
+
+const char PROGMEM str_baudrate_arg0[] = "baudrate";
+const parse_token_string_t PROGMEM cmd_baudrate_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_baudrate_result, arg0,
+ str_baudrate_arg0);
+const parse_token_num_t PROGMEM cmd_baudrate_arg1 =
+ TOKEN_NUM_INITIALIZER(struct cmd_baudrate_result, arg1,
+ UINT32);
+
+const char PROGMEM help_baudrate[] = "Change xbee baudrate";
+const parse_inst_t PROGMEM cmd_baudrate = {
+ .f = cmd_baudrate_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_baudrate,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_baudrate_arg0,
+ (PGM_P)&cmd_baudrate_arg1,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+
+/* this structure is filled when cmd_beep is parsed successfully */
+struct cmd_beep_result {
+ fixed_string_t beep;
+};
+
+/* function called when cmd_beep is parsed successfully */
+static void cmd_beep_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+
+ beep(0, 3, 3);
+ beep(1, 3, 3);
+ beep(2, 3, 3);
+ beep(0, 1, 1);
+ beep(1, 1, 1);
+ beep(2, 1, 1);
+}
+
+const char PROGMEM str_beep[] = "beep";
+const parse_token_string_t PROGMEM cmd_beep_beep =
+ TOKEN_STRING_INITIALIZER(struct cmd_beep_result, beep,
+ str_beep);
+
+const char PROGMEM help_beep[] = "Send a beep";
+
+const parse_inst_t PROGMEM cmd_beep = {
+ .f = cmd_beep_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_beep,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_beep_beep,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_servo is parsed successfully */
+struct cmd_servo_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint16_t num;
+ uint16_t val;
+};
+
+/* function called when cmd_servo is parsed successfully */
+static void cmd_servo_parsed(void * parsed_result, void *data)
+{
+ struct cmd_servo_result *res = parsed_result;
+
+ (void)data;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ if (res->num >= N_SERVO) {
+ printf_P(PSTR("bad servo num\n"));
+ return;
+ }
+ if (res->val >= 1024) {
+ printf_P(PSTR("bad servo val\n"));
+ return;
+ }
+ spi_servo_set(res->num, res->val);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("bypass"))) {
+ spi_servo_set_bypass(!!res->val);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("ppm"))) {
+ spi_servo_set_ppm(!!res->val);
+ }
+ else if (!strcmp_P(res->arg1, PSTR("show"))) {
+ spi_servo_dump();
+ }
+}
+
+const char PROGMEM str_servo_arg0[] = "servo";
+const parse_token_string_t PROGMEM cmd_servo_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg0,
+ str_servo_arg0);
+const char PROGMEM str_servo_arg1_set[] = "set";
+const parse_token_string_t PROGMEM cmd_servo_arg1_set =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1,
+ str_servo_arg1_set);
+const parse_token_num_t PROGMEM cmd_servo_num =
+ TOKEN_NUM_INITIALIZER(struct cmd_servo_result, num,
+ UINT16);
+const parse_token_num_t PROGMEM cmd_servo_val =
+ TOKEN_NUM_INITIALIZER(struct cmd_servo_result, val,
+ UINT16);
+
+const char PROGMEM help_servo_set[] = "set servo value";
+const parse_inst_t PROGMEM cmd_servo_set = {
+ .f = cmd_servo_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo_set,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_servo_arg0,
+ (PGM_P)&cmd_servo_arg1_set,
+ (PGM_P)&cmd_servo_num,
+ (PGM_P)&cmd_servo_val,
+ NULL,
+ },
+};
+
+const char PROGMEM str_servo_arg1_show[] = "show";
+const parse_token_string_t PROGMEM cmd_servo_arg1_show =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1,
+ str_servo_arg1_show);
+
+const char PROGMEM help_servo_show[] = "read servo and config";
+const parse_inst_t PROGMEM cmd_servo_show = {
+ .f = cmd_servo_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo_show,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_servo_arg0,
+ (PGM_P)&cmd_servo_arg1_show,
+ NULL,
+ },
+};
+
+const char PROGMEM str_servo_arg1_bypassppm[] = "bypass#ppm";
+const parse_token_string_t PROGMEM cmd_servo_arg1_bypassppm =
+ TOKEN_STRING_INITIALIZER(struct cmd_servo_result, arg1,
+ str_servo_arg1_bypassppm);
+
+const char PROGMEM help_servo_bypassppm[] = "change bypass/ppm";
+const parse_inst_t PROGMEM cmd_servo_bypassppm = {
+ .f = cmd_servo_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_servo_bypassppm,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_servo_arg0,
+ (PGM_P)&cmd_servo_arg1_bypassppm,
+ (PGM_P)&cmd_servo_val,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_test_spi is parsed successfully */
+struct cmd_test_spi_result {
+ fixed_string_t arg0;
+};
+
+static void cmd_test_spi_parsed(void * parsed_result, void *data)
+{
+ uint8_t i, flags, wait_time = 0;
+ uint16_t val = 0;
+
+ (void)parsed_result;
+ (void)data;
+
+ spi_servo_set_bypass(0);
+ spi_servo_set_ppm(0);
+
+ /* stress test: send many commands, no wait between each servo
+ * of a series, and a variable delay between series */
+ printf_P(PSTR("stress test\r\n"));
+ while (!cmdline_keypressed()) {
+
+ wait_time++;
+ if (wait_time > 20)
+ wait_time = 0;
+
+ IRQ_LOCK(flags);
+ val = global_ms;
+ IRQ_UNLOCK(flags);
+ val >>= 3;
+ val &= 1023;
+
+ for (i = 0; i < 6; i++)
+ spi_servo_set(i, val);
+
+ wait_ms(wait_time);
+ printf_P(PSTR("%4.4d %4.4d %4.4d %4.4d %4.4d %4.4d\r\n"),
+ spi_servo_get(0), spi_servo_get(1), spi_servo_get(2),
+ spi_servo_get(3), spi_servo_get(4), spi_servo_get(5));
+ }
+
+ printf_P(PSTR("bypass mode, with spi commands in background\r\n"));
+ spi_servo_set_bypass(1);
+
+ /* test bypass mode */
+ while (!cmdline_keypressed()) {
+
+ wait_time++;
+ if (wait_time > 20)
+ wait_time = 0;
+
+ IRQ_LOCK(flags);
+ val = global_ms;
+ IRQ_UNLOCK(flags);
+ val >>= 3;
+ val &= 1023;
+
+ for (i = 0; i < 6; i++)
+ spi_servo_set(i, val);
+
+ wait_ms(wait_time);
+ printf_P(PSTR("%4.4d %4.4d %4.4d %4.4d %4.4d %4.4d\r\n"),
+ spi_servo_get(0), spi_servo_get(1), spi_servo_get(2),
+ spi_servo_get(3), spi_servo_get(4), spi_servo_get(5));
+ }
+
+ printf_P(PSTR("PPM to servo\r\n"));
+ spi_servo_set_bypass(0);
+ spi_servo_set_ppm(0);
+
+ /* test PPM to servo (bypass) mode */
+ while (!cmdline_keypressed()) {
+ for (i = 0; i < 6; i++) {
+ val = spi_servo_get(i);
+ spi_servo_set(i, val);
+ }
+ }
+
+ printf_P(PSTR("PPM to (servo + PPM)\r\n"));
+ spi_servo_set_bypass(0);
+ spi_servo_set_ppm(1);
+
+ /* test PPM to servo (bypass) mode */
+ while (!cmdline_keypressed()) {
+ for (i = 0; i < 6; i++) {
+ val = spi_servo_get(i);
+ spi_servo_set(i, val);
+ }
+ }
+}
+
+const char PROGMEM str_test_spi_arg0[] = "test_spi";
+const parse_token_string_t PROGMEM cmd_test_spi_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_test_spi_result, arg0,
+ str_test_spi_arg0);
+
+const char PROGMEM help_test_spi[] = "Test the spi";
+const parse_inst_t PROGMEM cmd_test_spi = {
+ .f = cmd_test_spi_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_test_spi,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_test_spi_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_dump_xbee_stats is parsed successfully */
+struct cmd_dump_xbee_stats_result {
+ fixed_string_t arg0;
+};
+
+static void cmd_dump_xbee_stats_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+
+ xbee_dump_stats(xbee_dev);
+}
+
+const char PROGMEM str_dump_xbee_stats_arg0[] = "dump_xbee_stats";
+const parse_token_string_t PROGMEM cmd_dump_xbee_stats_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_dump_xbee_stats_result, arg0,
+ str_dump_xbee_stats_arg0);
+
+const char PROGMEM help_dump_xbee_stats[] = "Test the spi";
+const parse_inst_t PROGMEM cmd_dump_xbee_stats = {
+ .f = cmd_dump_xbee_stats_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_dump_xbee_stats,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_dump_xbee_stats_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_rc_proto_stats is parsed successfully */
+struct cmd_rc_proto_stats_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+static void cmd_rc_proto_stats_parsed(void *parsed_result, void *data)
+{
+ struct cmd_rc_proto_stats_result *res = parsed_result;
+ (void)data;
+
+ if (!strcmp(res->arg1, "show"))
+ rc_proto_dump_stats();
+ else /* reset */
+ rc_proto_reset_stats();
+}
+
+const char PROGMEM str_rc_proto_stats_arg0[] = "rc_proto_stats";
+const parse_token_string_t PROGMEM cmd_rc_proto_stats_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_stats_result, arg0,
+ str_rc_proto_stats_arg0);
+const char PROGMEM str_rc_proto_stats_arg1[] = "show#reset";
+const parse_token_string_t PROGMEM cmd_rc_proto_stats_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_stats_result, arg1,
+ str_rc_proto_stats_arg1);
+
+const char PROGMEM help_rc_proto_stats[] = "dump rc_proto stats";
+const parse_inst_t PROGMEM cmd_rc_proto_stats = {
+ .f = cmd_rc_proto_stats_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_stats,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_stats_arg0,
+ (PGM_P)&cmd_rc_proto_stats_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_rc_proto_timers is parsed successfully */
+struct cmd_rc_proto_timers_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint16_t servo_min;
+ uint16_t servo_max;
+ uint16_t power_probe;
+ uint16_t autobypass;
+};
+
+static void cmd_rc_proto_timers_parsed(void *parsed_result, void *data)
+{
+ struct cmd_rc_proto_timers_result *res = parsed_result;
+ (void)data;
+
+ if (!strcmp_P(res->arg1, PSTR("set"))) {
+ rc_proto_timers.send_servo_min_ms = res->servo_min;
+ rc_proto_timers.send_servo_max_ms = res->servo_max;
+ rc_proto_timers.send_power_probe_ms = res->power_probe;
+ rc_proto_timers.autobypass_ms = res->autobypass;
+ }
+
+ printf_P(PSTR("rc_proto_timers: min=%d, max=%d, "
+ "power_probe=%d autobypass=%d\n"),
+ rc_proto_timers.send_servo_min_ms,
+ rc_proto_timers.send_servo_max_ms,
+ rc_proto_timers.send_power_probe_ms,
+ rc_proto_timers.autobypass_ms);
+}
+
+const char PROGMEM str_rc_proto_timers_arg0[] = "rc_proto_timers";
+const parse_token_string_t PROGMEM cmd_rc_proto_timers_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_timers_result, arg0,
+ str_rc_proto_timers_arg0);
+const char PROGMEM str_rc_proto_timers_arg1[] = "set";
+const parse_token_string_t PROGMEM cmd_rc_proto_timers_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_timers_result, arg1,
+ str_rc_proto_timers_arg1);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_servo_min =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, servo_min,
+ UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_servo_max =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, servo_max,
+ UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_power_probe =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, power_probe,
+ UINT16);
+const parse_token_num_t PROGMEM cmd_rc_proto_timers_autobypass =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_timers_result, autobypass,
+ UINT16);
+
+const char PROGMEM help_rc_proto_timers[] = "set rc_proto_timers (servo_min, "
+ "servo_max, pow_probe, autobypass)";
+const parse_inst_t PROGMEM cmd_rc_proto_timers = {
+ .f = cmd_rc_proto_timers_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_timers,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_timers_arg0,
+ (PGM_P)&cmd_rc_proto_timers_arg1,
+ (PGM_P)&cmd_rc_proto_timers_servo_min,
+ (PGM_P)&cmd_rc_proto_timers_servo_max,
+ (PGM_P)&cmd_rc_proto_timers_power_probe,
+ (PGM_P)&cmd_rc_proto_timers_autobypass,
+ NULL,
+ },
+};
+
+const char PROGMEM str_rc_proto_timers_show_arg1[] = "show";
+const parse_token_string_t PROGMEM cmd_rc_proto_timers_show_arg1 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_timers_result, arg1,
+ str_rc_proto_timers_show_arg1);
+
+const char PROGMEM help_rc_proto_timers_show[] = "show rc_proto timers value";
+const parse_inst_t PROGMEM cmd_rc_proto_timers_show = {
+ .f = cmd_rc_proto_timers_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_timers_show,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_timers_arg0,
+ (PGM_P)&cmd_rc_proto_timers_show_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_rc_proto_mode is parsed successfully */
+struct cmd_rc_proto_mode_result {
+ fixed_string_t arg0;
+ fixed_string_t cmd;
+ fixed_string_t val;
+};
+
+static void cmd_rc_proto_mode_parsed(void *parsed_result, void *data)
+{
+ struct cmd_rc_proto_mode_result *res = parsed_result;
+ (void)data;
+ uint8_t flags;
+ uint8_t on = 0;
+
+ flags = rc_proto_get_mode();
+ if (!strcmp_P(res->val, PSTR("on")))
+ on = 1;
+
+ if (!strcmp_P(res->cmd, PSTR("rx_copy_spi"))) {
+ if (on == 1)
+ flags |= RC_PROTO_FLAGS_RX_COPY_SPI;
+ else
+ flags &= ~RC_PROTO_FLAGS_RX_COPY_SPI;
+ }
+ else if (!strcmp_P(res->cmd, PSTR("rx_autobypass"))) {
+ if (on == 1)
+ flags |= RC_PROTO_FLAGS_RX_AUTOBYPASS;
+ else
+ flags &= ~RC_PROTO_FLAGS_RX_AUTOBYPASS;
+ }
+ else if (!strcmp_P(res->cmd, PSTR("tx_stats"))) {
+ if (on == 1)
+ flags |= RC_PROTO_FLAGS_TX_STATS;
+ else
+ flags &= ~RC_PROTO_FLAGS_TX_STATS;
+ }
+ else if (!strcmp_P(res->cmd, PSTR("tx_power_probe"))) {
+ if (on == 1)
+ flags |= RC_PROTO_FLAGS_TX_POW_PROBE;
+ else
+ flags &= ~RC_PROTO_FLAGS_TX_POW_PROBE;
+ }
+ else if (!strcmp_P(res->cmd, PSTR("compute_best_pow"))) {
+ if (on == 1)
+ flags |= RC_PROTO_FLAGS_COMPUTE_BEST_POW;
+ else
+ flags &= ~RC_PROTO_FLAGS_COMPUTE_BEST_POW;
+ }
+ else if (!strcmp_P(res->cmd, PSTR("tx"))) {
+ flags &= ~RC_PROTO_FLAGS_TX_MASK;
+ if (!strcmp_P(res->val, PSTR("bypass")))
+ flags |= RC_PROTO_FLAGS_TX_BYPASS;
+ else if (!strcmp_P(res->val, PSTR("copy_spi")))
+ flags |= RC_PROTO_FLAGS_TX_COPY_SPI;
+ }
+ rc_proto_set_mode(flags);
+
+ /* dump state */
+ if ((flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_OFF)
+ printf_P(PSTR("rc_proto_mode tx off\n"));
+ else if ((flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_BYPASS)
+ printf_P(PSTR("rc_proto_mode tx bypass\n"));
+ else if ((flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_COPY_SPI)
+ printf_P(PSTR("rc_proto_mode tx copy_spi\n"));
+ printf_P(PSTR("rc_proto_mode rx_copy_spi %s\n"),
+ (flags & RC_PROTO_FLAGS_RX_COPY_SPI) ? "on" : "off");
+ printf_P(PSTR("rc_proto_mode rx_autobypass %s\n"),
+ (flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) ? "on" : "off");
+ printf_P(PSTR("rc_proto_mode tx_stats %s\n"),
+ (flags & RC_PROTO_FLAGS_TX_STATS) ? "on" : "off");
+ printf_P(PSTR("rc_proto_mode tx_power_probe %s\n"),
+ (flags & RC_PROTO_FLAGS_TX_POW_PROBE) ? "on" : "off");
+ printf_P(PSTR("rc_proto_mode compute_best_pow %s\n"),
+ (flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) ? "on" : "off");
+}
+
+const char PROGMEM str_rc_proto_mode_arg0[] = "rc_proto_mode";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, arg0,
+ str_rc_proto_mode_arg0);
+
+const char PROGMEM str_rc_proto_mode_cmd[] =
+ "rx_copy_spi#rx_autobypass#tx_stats#tx_power_probe#compute_best_pow";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd,
+ str_rc_proto_mode_cmd);
+
+const char PROGMEM str_rc_proto_mode_onoff[] = "on#off";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_onoff =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, val,
+ str_rc_proto_mode_onoff);
+
+const char PROGMEM help_rc_proto_mode[] = "Set rc proto behavior";
+const parse_inst_t PROGMEM cmd_rc_proto_mode = {
+ .f = cmd_rc_proto_mode_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_mode,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_mode_arg0,
+ (PGM_P)&cmd_rc_proto_mode_cmd,
+ (PGM_P)&cmd_rc_proto_mode_onoff,
+ NULL,
+ },
+};
+
+const char PROGMEM str_rc_proto_mode_cmd2[] = "tx";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd2 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd,
+ str_rc_proto_mode_cmd2);
+
+const char PROGMEM str_rc_proto_mode_val[] = "off#bypass#copy_spi";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_val =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, val,
+ str_rc_proto_mode_val);
+
+const parse_inst_t PROGMEM cmd_rc_proto_mode2 = {
+ .f = cmd_rc_proto_mode_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_mode,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_mode_arg0,
+ (PGM_P)&cmd_rc_proto_mode_cmd2,
+ (PGM_P)&cmd_rc_proto_mode_val,
+ NULL,
+ },
+};
+
+const char PROGMEM str_rc_proto_mode_cmd3[] = "show";
+const parse_token_string_t PROGMEM cmd_rc_proto_mode_cmd3 =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_mode_result, cmd,
+ str_rc_proto_mode_cmd3);
+
+const parse_inst_t PROGMEM cmd_rc_proto_mode3 = {
+ .f = cmd_rc_proto_mode_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_mode,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_mode_arg0,
+ (PGM_P)&cmd_rc_proto_mode_cmd3,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_rc_proto_hello is parsed successfully */
+struct cmd_rc_proto_hello_result {
+ fixed_string_t rc_proto_hello;
+ uint64_t addr;
+ struct xbee_neigh *neigh;
+ uint16_t period;
+ uint16_t count;
+ fixed_string_t data;
+};
+
+/* function called when cmd_rc_proto_hello is parsed successfully */
+static void cmd_rc_proto_hello_parsed(void *parsed_result, void *use_neigh)
+{
+ struct cmd_rc_proto_hello_result *res = parsed_result;
+ uint16_t now, next, diff;
+ uint8_t flags;
+ uint64_t addr;
+
+ if (use_neigh)
+ addr = res->neigh->addr;
+ else
+ addr = res->addr;
+
+ IRQ_LOCK(flags);
+ now = global_ms;
+ IRQ_UNLOCK(flags);
+
+ next = now;
+
+ while (!cmdline_keypressed() && res->count != 0) {
+ IRQ_LOCK(flags);
+ now = global_ms;
+ IRQ_UNLOCK(flags);
+
+ diff = now - next;
+ if (diff < res->period)
+ continue;
+
+ rc_proto_send_hello(addr, res->data, strlen(res->data), -1);
+ next += res->period;
+ res->count--;
+ }
+}
+
+const char PROGMEM str_rc_proto_hello[] = "rc_proto_hello";
+
+const parse_token_string_t PROGMEM cmd_rc_proto_hello_rc_proto_hello =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_hello_result, rc_proto_hello,
+ str_rc_proto_hello);
+
+const parse_token_num_t PROGMEM cmd_rc_proto_hello_addr =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_hello_result, addr, UINT64);
+
+const parse_token_num_t PROGMEM cmd_rc_proto_hello_period =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_hello_result, period, UINT16);
+
+const parse_token_num_t PROGMEM cmd_rc_proto_hello_count =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_hello_result, count, UINT16);
+
+const parse_token_string_t PROGMEM cmd_rc_proto_hello_data =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_hello_result, data, NULL);
+
+const char PROGMEM help_rc_proto_hello[] =
+ "Send hello msg to a node: addr, period_ms, count, str";
+
+const parse_inst_t PROGMEM cmd_rc_proto_hello = {
+ .f = cmd_rc_proto_hello_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_hello,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_hello_rc_proto_hello,
+ (PGM_P)&cmd_rc_proto_hello_addr,
+ (PGM_P)&cmd_rc_proto_hello_period,
+ (PGM_P)&cmd_rc_proto_hello_count,
+ (PGM_P)&cmd_rc_proto_hello_data,
+ NULL,
+ },
+};
+
+const parse_token_neighbor_t PROGMEM cmd_rc_proto_hello_neigh =
+ TOKEN_NEIGHBOR_INITIALIZER(struct cmd_rc_proto_hello_result, neigh,
+ &xbee_dev);
+
+const parse_inst_t PROGMEM cmd_rc_proto_hello_name = {
+ .f = cmd_rc_proto_hello_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_rc_proto_hello,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_hello_rc_proto_hello,
+ (PGM_P)&cmd_rc_proto_hello_neigh,
+ (PGM_P)&cmd_rc_proto_hello_period,
+ (PGM_P)&cmd_rc_proto_hello_count,
+ (PGM_P)&cmd_rc_proto_hello_data,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_rc_proto_echo is parsed successfully */
+struct cmd_rc_proto_echo_result {
+ fixed_string_t rc_proto_echo;
+ uint64_t addr;
+ struct xbee_neigh *neigh;
+ uint16_t period;
+ uint16_t count;
+ fixed_string_t data;
+};
+
+/* function called when cmd_rc_proto_echo is parsed successfully */
+static void cmd_rc_proto_echo_parsed(void *parsed_result, void *use_neigh)
+{
+ struct cmd_rc_proto_echo_result *res = parsed_result;
+ uint16_t now, next, diff;
+ uint8_t flags;
+ uint64_t addr;
+
+ if (use_neigh)
+ addr = res->neigh->addr;
+ else
+ addr = res->addr;
+
+ IRQ_LOCK(flags);
+ now = global_ms;
+ IRQ_UNLOCK(flags);
+
+ next = now;
+
+ while (!cmdline_keypressed() && res->count != 0) {
+ IRQ_LOCK(flags);
+ now = global_ms;
+ IRQ_UNLOCK(flags);
+
+ diff = now - next;
+ if (diff < res->period)
+ continue;
+
+ rc_proto_send_echo_req(addr, res->data, strlen(res->data), -1);
+ next += res->period;
+ res->count--;
+ }
+}
+
+const char PROGMEM str_rc_proto_echo[] = "rc_proto_echo";
+
+const parse_token_string_t PROGMEM cmd_rc_proto_echo_rc_proto_echo =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_echo_result, rc_proto_echo,
+ str_rc_proto_echo);
+
+const parse_token_num_t PROGMEM cmd_rc_proto_echo_addr =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_echo_result, addr, UINT64);
+
+const parse_token_num_t PROGMEM cmd_rc_proto_echo_period =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_echo_result, period, UINT16);
+
+const parse_token_num_t PROGMEM cmd_rc_proto_echo_count =
+ TOKEN_NUM_INITIALIZER(struct cmd_rc_proto_echo_result, count, UINT16);
+
+const parse_token_string_t PROGMEM cmd_rc_proto_echo_data =
+ TOKEN_STRING_INITIALIZER(struct cmd_rc_proto_echo_result, data, NULL);
+
+const char PROGMEM help_rc_proto_echo[] =
+ "Send echo msg to a node: addr, period_ms, count, str";
+
+const parse_inst_t PROGMEM cmd_rc_proto_echo = {
+ .f = cmd_rc_proto_echo_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_rc_proto_echo,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_echo_rc_proto_echo,
+ (PGM_P)&cmd_rc_proto_echo_addr,
+ (PGM_P)&cmd_rc_proto_echo_period,
+ (PGM_P)&cmd_rc_proto_echo_count,
+ (PGM_P)&cmd_rc_proto_echo_data,
+ NULL,
+ },
+};
+
+const parse_token_neighbor_t PROGMEM cmd_rc_proto_echo_neigh =
+ TOKEN_NEIGHBOR_INITIALIZER(struct cmd_rc_proto_echo_result, neigh,
+ &xbee_dev);
+
+const parse_inst_t PROGMEM cmd_rc_proto_echo_name = {
+ .f = cmd_rc_proto_echo_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_rc_proto_echo,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_rc_proto_echo_rc_proto_echo,
+ (PGM_P)&cmd_rc_proto_echo_neigh,
+ (PGM_P)&cmd_rc_proto_echo_period,
+ (PGM_P)&cmd_rc_proto_echo_count,
+ (PGM_P)&cmd_rc_proto_echo_data,
+ NULL,
+ },
+};
+
+/**********************************************************/
+
+/* this structure is filled when cmd_test_eeprom_config is parsed successfully */
+struct cmd_test_eeprom_config_result {
+ fixed_string_t arg0;
+};
+
+static void cmd_test_eeprom_config_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+
+ eeprom_dump_cmds();
+ eeprom_append_cmd("salut1\n");
+ eeprom_dump_cmds();
+ eeprom_append_cmd("salut2\n");
+ eeprom_append_cmd("salut3\n");
+ eeprom_append_cmd("salut4\n");
+ eeprom_dump_cmds();
+ eeprom_insert_cmd_before("coin\n", 0);
+ eeprom_insert_cmd_before("coin2\n", 2);
+ eeprom_dump_cmds();
+ eeprom_delete_cmd(2);
+ eeprom_delete_cmd(0);
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_test_eeprom_config_arg0[] = "test_eeprom_config";
+const parse_token_string_t PROGMEM cmd_test_eeprom_config_arg0 =
+ TOKEN_STRING_INITIALIZER(struct cmd_test_eeprom_config_result, arg0,
+ str_test_eeprom_config_arg0);
+
+const char PROGMEM help_test_eeprom_config[] = "Test the eeprom configuration";
+const parse_inst_t PROGMEM cmd_test_eeprom_config = {
+ .f = cmd_test_eeprom_config_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_test_eeprom_config,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_test_eeprom_config_arg0,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_eeprom_del_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+ uint8_t n;
+};
+
+static void cmd_eeprom_del_parsed(void *parsed_result,
+ void *data)
+{
+ struct cmd_eeprom_del_result *res = parsed_result;
+
+ (void)data;
+ if (eeprom_delete_cmd(res->n) < 0)
+ printf_P(PSTR("cannot delete command\n"));
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_eeprom_del_eeprom[] = "eeprom";
+const parse_token_string_t PROGMEM cmd_eeprom_del_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_del_result, cmd,
+ str_eeprom_del_eeprom);
+const char PROGMEM str_eeprom_del_del[] = "del";
+const parse_token_string_t PROGMEM cmd_eeprom_del_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_del_result, action,
+ str_eeprom_del_del);
+const parse_token_num_t PROGMEM cmd_eeprom_del_num =
+ TOKEN_NUM_INITIALIZER(struct cmd_eeprom_del_result, n,
+ UINT8);
+
+const char PROGMEM help_eeprom_del[] = "delete an eeprom init command";
+const parse_inst_t PROGMEM cmd_eeprom_del = {
+ .f = cmd_eeprom_del_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_eeprom_del,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_del_cmd,
+ (PGM_P)&cmd_eeprom_del_action,
+ (PGM_P)&cmd_eeprom_del_num,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_eeprom_add_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+ uint8_t n;
+};
+
+static void cmd_eeprom_add_parsed(void *parsed_result,
+ void *data)
+{
+ struct cmd_eeprom_add_result *res = parsed_result;
+ struct rdline rdl;
+ const char *buffer;
+ int8_t ret;
+ int16_t c;
+
+ rdline_init(&rdl, cmdline_write_char, NULL, NULL);
+ rdline_newline(&rdl, "> ");
+
+ while (1) {
+ c = cmdline_dev_recv(NULL);
+ if (c < 0)
+ continue;
+
+ ret = rdline_char_in(&rdl, c);
+ if (ret == -2) {
+ printf_P(PSTR("abort\n"));
+ return;
+ }
+ if (ret == 1)
+ break;
+ }
+
+ buffer = rdline_get_buffer(&rdl);
+ if (data == NULL)
+ eeprom_insert_cmd_before(buffer, res->n);
+ else
+ eeprom_append_cmd(buffer);
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_eeprom_add_eeprom[] = "eeprom";
+const parse_token_string_t PROGMEM cmd_eeprom_add_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_add_result, cmd,
+ str_eeprom_add_eeprom);
+const char PROGMEM str_eeprom_add_add[] = "add";
+const parse_token_string_t PROGMEM cmd_eeprom_add_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_add_result, action,
+ str_eeprom_add_add);
+const parse_token_num_t PROGMEM cmd_eeprom_add_num =
+ TOKEN_NUM_INITIALIZER(struct cmd_eeprom_add_result, n,
+ UINT8);
+
+const char PROGMEM help_eeprom_add[] = "insert an eeprom init command";
+const parse_inst_t PROGMEM cmd_eeprom_add = {
+ .f = cmd_eeprom_add_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_eeprom_add,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_add_cmd,
+ (PGM_P)&cmd_eeprom_add_action,
+ (PGM_P)&cmd_eeprom_add_num,
+ NULL,
+ },
+};
+
+const char PROGMEM help_eeprom_add2[] = "append an eeprom init command";
+const parse_inst_t PROGMEM cmd_eeprom_add2 = {
+ .f = cmd_eeprom_add_parsed, /* function to call */
+ .data = (void *)1, /* 2nd arg of func */
+ .help_str = help_eeprom_add2,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_add_cmd,
+ (PGM_P)&cmd_eeprom_add_action,
+ NULL,
+ },
+};
+
+/* ************* */
+
+struct cmd_eeprom_list_result {
+ fixed_string_t cmd;
+ fixed_string_t action;
+};
+
+static void cmd_eeprom_list_parsed(void *parsed_result,
+ void *data)
+{
+ (void)parsed_result;
+ (void)data;
+ eeprom_dump_cmds();
+}
+
+const char PROGMEM str_eeprom_list_eeprom[] = "eeprom";
+const parse_token_string_t PROGMEM cmd_eeprom_list_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_list_result, cmd,
+ str_eeprom_list_eeprom);
+const char PROGMEM str_eeprom_list_list[] = "list";
+const parse_token_string_t PROGMEM cmd_eeprom_list_action =
+ TOKEN_STRING_INITIALIZER(struct cmd_eeprom_list_result, action,
+ str_eeprom_list_list);
+
+const char PROGMEM help_eeprom_list[] = "list all eeprom init commands";
+const parse_inst_t PROGMEM cmd_eeprom_list = {
+ .f = cmd_eeprom_list_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_eeprom_list,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_eeprom_list_cmd,
+ (PGM_P)&cmd_eeprom_list_action,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+struct cmd_dump_i2c_result {
+ fixed_string_t cmd;
+};
+
+static void cmd_dump_i2c_parsed(void *parsed_result, void *data)
+{
+ struct i2c_ans_imuboard_status imu;
+ uint8_t irq_flags;
+
+ (void)parsed_result;
+ (void)data;
+
+ while (!cmdline_keypressed()) {
+ IRQ_LOCK(irq_flags);
+ memcpy(&imu, &imuboard_status, sizeof(imu));
+ IRQ_UNLOCK(irq_flags);
+
+ if (imu.flags & IMUBOARD_STATUS_GPS_OK) {
+ printf_P(PSTR("GPS lat=%"PRIi32" long=%"PRIi32
+ " alt=%"PRIi32"\n"),
+ imu.latitude, imu.longitude, imu.altitude);
+ }
+ else
+ printf_P(PSTR("GPS unavailable\n"));
+ i2c_protocol_debug();
+ wait_ms(100);
+ }
+}
+
+const char PROGMEM str_dump_i2c[] = "dump_i2c";
+const parse_token_string_t PROGMEM cmd_dump_i2c_cmd =
+ TOKEN_STRING_INITIALIZER(struct cmd_dump_i2c_result, cmd,
+ str_dump_i2c);
+
+const char PROGMEM help_dump_i2c[] = "dump_i2c";
+const parse_inst_t PROGMEM cmd_dump_i2c = {
+ .f = cmd_dump_i2c_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_dump_i2c,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_dump_i2c_cmd,
+ NULL,
+ },
+};
+
+
+/* ************* */
+
+/* in progmem */
+const parse_ctx_t PROGMEM main_ctx[] = {
+
+ /* commands_gen.c */
+ &cmd_reset,
+ &cmd_bootloader,
+ &cmd_log,
+ &cmd_log_show,
+ &cmd_log_type,
+ &cmd_stack_space,
+ &cmd_callout,
+ &cmd_help,
+ &cmd_neigh_del,
+ &cmd_neigh_add,
+ &cmd_neigh_list,
+ &cmd_read,
+ &cmd_write_none,
+ &cmd_write_u8,
+ &cmd_write_u16,
+ &cmd_write_u32,
+ &cmd_sendmsg,
+ &cmd_sendmsg_name,
+ &cmd_range,
+ &cmd_range_period,
+ &cmd_range_count,
+ &cmd_range_powermask,
+ &cmd_range_dstaddr,
+ &cmd_monitor,
+ &cmd_monitor_period,
+ &cmd_monitor_add,
+ &cmd_monitor_del,
+ &cmd_ping,
+ &cmd_raw,
+ &cmd_baudrate,
+ &cmd_beep,
+ &cmd_servo_set,
+ &cmd_servo_bypassppm,
+ &cmd_servo_show,
+ &cmd_test_spi,
+ &cmd_dump_xbee_stats,
+ &cmd_rc_proto_stats,
+ &cmd_rc_proto_timers,
+ &cmd_rc_proto_timers_show,
+ &cmd_rc_proto_mode,
+ &cmd_rc_proto_mode2,
+ &cmd_rc_proto_mode3,
+ &cmd_rc_proto_hello,
+ &cmd_rc_proto_hello_name,
+ &cmd_rc_proto_echo,
+ &cmd_rc_proto_echo_name,
+ &cmd_test_eeprom_config,
+ &cmd_eeprom_del,
+ &cmd_eeprom_add,
+ &cmd_eeprom_add2,
+ &cmd_eeprom_list,
+ &cmd_dump_i2c,
+ NULL,
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation (2011)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: commands_gen.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ * Olivier MATZ <zer0@droids-corp.org>
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+#include <aversive/queue.h>
+
+#include <uart.h>
+
+#include <rdline.h>
+#include <parse.h>
+#include <parse_string.h>
+#include <parse_num.h>
+
+#include <diagnostic.h>
+
+#include "callout.h"
+#include "main.h"
+#include "cmdline.h"
+
+/**********************************************************/
+/* Reset */
+
+/* this structure is filled when cmd_reset is parsed successfully */
+struct cmd_reset_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_reset is parsed successfully */
+static void cmd_reset_parsed(void * parsed_result, void * data)
+{
+ (void)parsed_result;
+ (void)data;
+#ifdef HOST_VERSION
+ hostsim_exit();
+#endif
+ reset();
+}
+
+const char PROGMEM str_reset_arg0[] = "reset";
+const parse_token_string_t PROGMEM cmd_reset_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_reset_result, arg0, str_reset_arg0);
+
+const char PROGMEM help_reset[] = "Reset the board";
+const parse_inst_t PROGMEM cmd_reset = {
+ .f = cmd_reset_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_reset,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_reset_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Bootloader */
+
+/* this structure is filled when cmd_bootloader is parsed successfully */
+struct cmd_bootloader_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_bootloader is parsed successfully */
+static void cmd_bootloader_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+#ifndef HOST_VERSION
+ bootloader();
+#else
+ printf("not implemented\n");
+#endif
+}
+
+const char PROGMEM str_bootloader_arg0[] = "bootloader";
+const parse_token_string_t PROGMEM cmd_bootloader_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_bootloader_result, arg0, str_bootloader_arg0);
+
+const char PROGMEM help_bootloader[] = "Launch the bootloader";
+const parse_inst_t PROGMEM cmd_bootloader = {
+ .f = cmd_bootloader_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_bootloader,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_bootloader_arg0,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Callout show */
+
+/* this structure is filled when cmd_callout is parsed successfully */
+struct cmd_callout_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+};
+
+/* function called when cmd_callout is parsed successfully */
+static void cmd_callout_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+ callout_dump_stats(&xbeeboard.intr_cm);
+}
+
+const char PROGMEM str_callout_arg0[] = "callout";
+const parse_token_string_t PROGMEM cmd_callout_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_callout_result, arg0, str_callout_arg0);
+const char PROGMEM str_callout_arg1[] = "show";
+const parse_token_string_t PROGMEM cmd_callout_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_callout_result, arg1, str_callout_arg1);
+
+const char PROGMEM help_callout[] = "Show callout events";
+const parse_inst_t PROGMEM cmd_callout = {
+ .f = cmd_callout_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_callout,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_callout_arg0,
+ (PGM_P)&cmd_callout_arg1,
+ NULL,
+ },
+};
+
+/**********************************************************/
+/* Log */
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ uint8_t arg2;
+ fixed_string_t arg3;
+};
+
+/* keep it sync with string choice */
+static const char PROGMEM uart_log[] = "uart";
+static const char PROGMEM i2c_log[] = "i2c";
+static const char PROGMEM default_log[] = "default";
+static const char PROGMEM xbee_log[] = "xbee";
+static const char PROGMEM rc_proto_log[] = "rc_proto";
+
+struct log_name_and_num {
+ const char *name;
+ uint8_t num;
+};
+
+static const struct log_name_and_num log_name_and_num[] = {
+ { uart_log, E_UART },
+ { i2c_log, E_I2C },
+ { default_log, E_USER_DEFAULT },
+ { xbee_log, E_USER_XBEE },
+ { rc_proto_log, E_USER_RC_PROTO },
+};
+
+static uint8_t
+log_name2num(const char * s)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (!strcmp_P(s, log_name_and_num[i].name)) {
+ return log_name_and_num[i].num;
+ }
+ }
+ return 0;
+}
+
+const char *
+log_num2name(uint8_t num)
+{
+ uint8_t i;
+
+ for (i=0; i<sizeof(log_name_and_num)/sizeof(struct log_name_and_num); i++) {
+ if (num == log_name_and_num[i].num) {
+ return log_name_and_num[i].name;
+ }
+ }
+ return NULL;
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_do_show(void)
+{
+ uint8_t i, empty=1;
+ const char *name;
+
+ printf_P(PSTR("log level is %d\r\n"), xbeeboard.log_level);
+ for (i=0; i<NB_LOGS; i++) {
+ name = log_num2name(xbeeboard.logs[i]);
+ if (name) {
+#ifdef HOST_VERSION
+ printf_P(PSTR("log type %s is on\r\n"), name);
+#else
+ printf_P(PSTR("log type %S is on\r\n"), name);
+#endif
+ empty = 0;
+ }
+ }
+ if (empty)
+ printf_P(PSTR("no log configured\r\n"));
+}
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_parsed(void * parsed_result, void *data)
+{
+ struct cmd_log_result *res = (struct cmd_log_result *) parsed_result;
+
+ (void)data;
+ if (!strcmp_P(res->arg1, PSTR("level"))) {
+ xbeeboard.log_level = res->arg2;
+ }
+
+ /* else it is a show */
+ cmd_log_do_show();
+}
+
+const char PROGMEM str_log_arg0[] = "log";
+const parse_token_string_t PROGMEM cmd_log_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg0, str_log_arg0);
+const char PROGMEM str_log_arg1[] = "level";
+const parse_token_string_t PROGMEM cmd_log_arg1 = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1);
+const parse_token_num_t PROGMEM cmd_log_arg2 = TOKEN_NUM_INITIALIZER(struct cmd_log_result, arg2, INT8);
+
+const char PROGMEM help_log[] = "Set log options: level (0 -> 5)";
+const parse_inst_t PROGMEM cmd_log = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_log_arg0,
+ (PGM_P)&cmd_log_arg1,
+ (PGM_P)&cmd_log_arg2,
+ NULL,
+ },
+};
+
+const char PROGMEM str_log_arg1_show[] = "show";
+const parse_token_string_t PROGMEM cmd_log_arg1_show = TOKEN_STRING_INITIALIZER(struct cmd_log_result, arg1, str_log_arg1_show);
+
+const char PROGMEM help_log_show[] = "Show configured logs";
+const parse_inst_t PROGMEM cmd_log_show = {
+ .f = cmd_log_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_show,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_log_arg0,
+ (PGM_P)&cmd_log_arg1_show,
+ NULL,
+ },
+};
+
+/* this structure is filled when cmd_log is parsed successfully */
+struct cmd_log_type_result {
+ fixed_string_t arg0;
+ fixed_string_t arg1;
+ fixed_string_t arg2;
+ fixed_string_t arg3;
+};
+
+/* function called when cmd_log is parsed successfully */
+static void cmd_log_type_parsed(void * parsed_result, void *data)
+{
+ struct cmd_log_type_result *res = (struct cmd_log_type_result *) parsed_result;
+ uint8_t lognum;
+ uint8_t i;
+
+ (void)data;
+
+ lognum = log_name2num(res->arg2);
+ if (lognum == 0) {
+ printf_P(PSTR("Cannot find log num\r\n"));
+ return;
+ }
+
+ if (!strcmp_P(res->arg3, PSTR("on"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (xbeeboard.logs[i] == lognum) {
+ printf_P(PSTR("Already on\r\n"));
+ return;
+ }
+ }
+ for (i=0; i<NB_LOGS; i++) {
+ if (xbeeboard.logs[i] == 0) {
+ xbeeboard.logs[i] = lognum;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("no more room\r\n"));
+ }
+ }
+ else if (!strcmp_P(res->arg3, PSTR("off"))) {
+ for (i=0; i<NB_LOGS; i++) {
+ if (xbeeboard.logs[i] == lognum) {
+ xbeeboard.logs[i] = 0;
+ break;
+ }
+ }
+ if (i==NB_LOGS) {
+ printf_P(PSTR("already off\r\n"));
+ }
+ }
+ cmd_log_do_show();
+}
+
+const char PROGMEM str_log_arg1_type[] = "type";
+const parse_token_string_t PROGMEM cmd_log_arg1_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg1, str_log_arg1_type);
+/* keep it sync with log_name_and_num above */
+const char PROGMEM str_log_arg2_type[] =
+ "uart#i2c#i2cproto#default#xbee#rc_proto";
+const parse_token_string_t PROGMEM cmd_log_arg2_type = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg2, str_log_arg2_type);
+const char PROGMEM str_log_arg3[] = "on#off";
+const parse_token_string_t PROGMEM cmd_log_arg3 = TOKEN_STRING_INITIALIZER(struct cmd_log_type_result, arg3, str_log_arg3);
+
+const char PROGMEM help_log_type[] = "Set log type";
+const parse_inst_t PROGMEM cmd_log_type = {
+ .f = cmd_log_type_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_log_type,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_log_arg0,
+ (PGM_P)&cmd_log_arg1_type,
+ (PGM_P)&cmd_log_arg2_type,
+ (PGM_P)&cmd_log_arg3,
+ NULL,
+ },
+};
+
+
+/**********************************************************/
+/* Stack_Space */
+
+/* this structure is filled when cmd_stack_space is parsed successfully */
+struct cmd_stack_space_result {
+ fixed_string_t arg0;
+};
+
+/* function called when cmd_stack_space is parsed successfully */
+static void cmd_stack_space_parsed(void *parsed_result, void *data)
+{
+ (void)parsed_result;
+ (void)data;
+#ifdef HOST_VERSION
+ printf("not implemented\n");
+#else
+ printf("res stack: %d\r\n", min_stack_space_available());
+#endif
+}
+
+const char PROGMEM str_stack_space_arg0[] = "stack_space";
+const parse_token_string_t PROGMEM cmd_stack_space_arg0 = TOKEN_STRING_INITIALIZER(struct cmd_stack_space_result, arg0, str_stack_space_arg0);
+
+const char PROGMEM help_stack_space[] = "Display remaining stack space";
+const parse_inst_t PROGMEM cmd_stack_space = {
+ .f = cmd_stack_space_parsed, /* function to call */
+ .data = NULL, /* 2nd arg of func */
+ .help_str = help_stack_space,
+ .tokens = { /* token list, NULL terminated */
+ (PGM_P)&cmd_stack_space_arg0,
+ NULL,
+ },
+};
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: diagnostic_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _DEBUG_CONFIG_
+#define _DEBUG_CONFIG_ 1.0 // version
+
+
+/** port line definition for the show_int_loop() function */
+/* undefine it to disable this functionnality */
+#define INTERRUPT_SHOW_PORT PORTA
+#define INTERRUPT_SHOW_BIT 3
+
+
+
+/** memory mark for the min_stack_space_available() function
+ the ram is filled with this value after a reset ! */
+#define MARK 0x55
+
+/** the mark is inserted in whole RAM if this is enabled
+ (could lead to problems if you need to hold values through a reset...)
+ so it's better to disable it.
+ stack counting is not affected */
+//#define DIAG_FILL_ENTIRE_RAM
+
+
+#endif //_DEBUG_CONFIG_
--- /dev/null
+#! /usr/bin/env python
+
+import os,sys,termios,atexit
+import serial
+from select import select
+import cmd
+#import pylab
+from matplotlib import pylab
+from math import *
+
+import struct
+import numpy
+import shlex
+import time
+import math
+import warnings
+warnings.filterwarnings("ignore","tempnam",RuntimeWarning, __name__)
+
+import logging
+log = logging.getLogger("XbeeShell")
+_handler = logging.StreamHandler()
+_handler.setFormatter(logging.Formatter("%(levelname)s: %(message)s"))
+log.addHandler(_handler)
+log.setLevel(1)
+
+class SerialLogger:
+ def __init__(self, ser, filein, fileout=None):
+ self.ser = ser
+ self.filein = filein
+ self.fin = open(filein, "a", 0)
+ if fileout:
+ self.fileout = fileout
+ self.fout = open(fileout, "a", 0)
+ else:
+ self.fileout = filein
+ self.fout = self.fin
+ def fileno(self):
+ return self.ser.fileno()
+ def read(self, *args):
+ res = self.ser.read(*args)
+ self.fin.write(res)
+ return res
+ def write(self, s):
+ self.fout.write(s)
+ self.ser.write(s)
+ def readline(self, *args):
+ res = self.ser.readline(*args)
+ self.fin.write(res)
+ return res
+
+class Interp(cmd.Cmd):
+ prompt = "Xbee> "
+ def __init__(self, tty, baudrate=57600):
+ cmd.Cmd.__init__(self)
+ self.ser = serial.Serial(tty,baudrate=baudrate,timeout=0.1)
+ self.escape = "\x01" # C-a
+ self.quitraw = "\x02" # C-b
+ self.serial_logging = False
+ self.default_in_log_file = "/tmp/xbee.in.log"
+ self.default_out_log_file = "/tmp/xbee.out.log"
+
+ def do_log(self, args):
+ """Activate serial logs.
+ log <filename> logs input and output to <filename>
+ log <filein> <fileout> logs input to <filein> and output to <fileout>
+ log logs to /tmp/microb.log or the last used file"""
+
+ if self.serial_logging:
+ log.error("Already logging to %s and %s" % (self.ser.filein,
+ self.ser.fileout))
+ else:
+ self.serial_logging = True
+ files = [os.path.expanduser(x) for x in args.split()]
+ if len(files) == 0:
+ files = [self.default_in_log_file, self.default_out_log_file]
+ elif len(files) == 1:
+ self.default_in_log_file = files[0]
+ self.default_out_log_file = None
+ elif len(files) == 2:
+ self.default_in_log_file = files[0]
+ self.default_out_log_file = files[1]
+ else:
+ print "Can't parse arguments"
+
+ self.ser = SerialLogger(self.ser, *files)
+ log.info("Starting serial logging to %s and %s" % (self.ser.filein,
+ self.ser.fileout))
+
+
+ def do_unlog(self, args):
+ if self.serial_logging:
+ log.info("Stopping serial logging to %s and %s" % (self.ser.filein,
+ self.ser.fileout))
+ self.ser = self.ser.ser
+ self.serial_logging = False
+ else:
+ log.error("No log to stop")
+
+ def do_raw(self, args):
+ "Switch to RAW mode"
+ stdin = os.open("/dev/stdin",os.O_RDONLY)
+ stdout = os.open("/dev/stdout",os.O_WRONLY)
+
+ stdin_termios = termios.tcgetattr(stdin)
+ raw_termios = stdin_termios[:]
+
+ try:
+ log.info("Switching to RAW mode")
+
+ # iflag
+ raw_termios[0] &= ~(termios.IGNBRK | termios.BRKINT |
+ termios.PARMRK | termios.ISTRIP |
+ termios.INLCR | termios.IGNCR |
+ termios.ICRNL | termios.IXON)
+ # oflag
+ raw_termios[1] &= ~termios.OPOST;
+ # cflag
+ raw_termios[2] &= ~(termios.CSIZE | termios.PARENB);
+ raw_termios[2] |= termios.CS8;
+ # lflag
+ raw_termios[3] &= ~(termios.ECHO | termios.ECHONL |
+ termios.ICANON | termios.ISIG |
+ termios.IEXTEN);
+
+ termios.tcsetattr(stdin, termios.TCSADRAIN, raw_termios)
+
+ mode = "normal"
+ while True:
+ ins,outs,errs=select([stdin,self.ser],[],[])
+ for x in ins:
+ if x == stdin:
+ c = os.read(stdin,1)
+ if mode == "escape":
+ mode =="normal"
+ if c == self.escape:
+ self.ser.write(self.escape)
+ elif c == self.quitraw:
+ return
+ else:
+ self.ser.write(self.escape)
+ self.ser.write(c)
+ else:
+ if c == self.escape:
+ mode = "escape"
+ else:
+ self.ser.write(c)
+ elif x == self.ser:
+ os.write(stdout,self.ser.read())
+ finally:
+ termios.tcsetattr(stdin, termios.TCSADRAIN, stdin_termios)
+ log.info("Back to normal mode")
+
+ def do_dump_stats(self, args):
+ """send hello at power 4 and dump stats regulary"""
+
+ self.ser.write("write power-level 4\n")
+ prev_reset = time.time()
+ try:
+ while True:
+ # send hellos during 1 s
+ self.ser.write("rc_proto_hello wing 30 30 tototiti\n")
+ time.sleep(1.5)
+
+ # send a reset every 60 s
+ t = time.time()
+ if t - prev_reset > 60:
+ self.ser.write("write soft-reset\n")
+ prev_reset = t
+
+ # display time & stats
+ self.ser.fin.write("\nTIME:%s\n"%(time.time()))
+ self.ser.write("rc_proto_stats show\n")
+ while True:
+ l = self.ser.readline()
+ sys.stdout.write(l)
+ if l == "":
+ break
+ except KeyboardInterrupt:
+ pass
+
+
+if __name__ == "__main__":
+ try:
+ import readline,atexit
+ except ImportError:
+ pass
+ else:
+ histfile = os.path.join(os.environ["HOME"], ".xbee")
+ atexit.register(readline.write_history_file, histfile)
+ try:
+ readline.read_history_file(histfile)
+ except IOError:
+ pass
+
+ device = "/dev/ttyS0"
+ if len(sys.argv) > 1:
+ device = sys.argv[1]
+ interp = Interp(device)
+ interp.do_log("")
+ while 1:
+ try:
+ interp.cmdloop()
+ except KeyboardInterrupt:
+ print
+ except Exception,e:
+ l = str(e).strip()
+ if l:
+ log.exception("%s" % l.splitlines()[-1])
+ continue
+ break
--- /dev/null
+/*
+ * Copyright 2013 Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include <stdint.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/eeprom.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+
+
+#include "cmdline.h"
+#include "eeprom_config.h"
+
+/* load configuration from eeprom */
+int8_t eeprom_load_config(void)
+{
+ struct eeprom_config *e = NULL;
+ struct eeprom_cmd cmd;
+ uint32_t magic;
+ uint8_t i, max;
+
+ eeprom_read_block(&magic, &e->magic, sizeof(magic));
+ if (magic != EEPROM_CONFIG_MAGIC) {
+ printf_P(PSTR("no EEPROM config\n"));
+ eeprom_set_ncmds(0);
+ return 0;
+ }
+
+ max = eeprom_get_ncmds();
+ for (i = 0; i < max; i++) {
+ eeprom_get_cmd(&cmd, i);
+ printf_P(PSTR("%s"), cmd.buf);
+ cmdline_valid_buffer(cmd.buf, strlen(cmd.buf));
+ }
+
+ return -1;
+}
+
+uint8_t eeprom_get_ncmds(void)
+{
+ struct eeprom_config *e = NULL;
+ uint8_t ncmds;
+
+ eeprom_read_block(&ncmds, &e->ncmds, sizeof(ncmds));
+ return ncmds;
+}
+
+void eeprom_set_ncmds(uint8_t ncmds)
+{
+ struct eeprom_config *e = NULL;
+ uint32_t magic = EEPROM_CONFIG_MAGIC;
+ eeprom_update_block(&ncmds, &e->ncmds, sizeof(ncmds));
+ eeprom_update_block(&magic, &e->magic, sizeof(magic));
+}
+
+/* fill cmd struct with the n-th command from eeprom, no check is done
+ * on index or size. The \0 is added at the end of the string. */
+void eeprom_get_cmd(struct eeprom_cmd *cmd, uint8_t n)
+{
+ struct eeprom_config *e = NULL;
+
+ eeprom_read_block(cmd, &e->cmds[n], sizeof(*cmd));
+ cmd->buf[EEPROM_CMD_SIZE-1] = '\0';
+}
+
+/* fill n-th command of eeprom from struct, no check is done on index
+ * or size */
+void eeprom_set_cmd(struct eeprom_cmd *cmd, uint8_t n)
+{
+ struct eeprom_config *e = NULL;
+
+ eeprom_update_block(cmd, &e->cmds[n], sizeof(*cmd));
+}
+
+void eeprom_dump_cmds(void)
+{
+ uint8_t i, max;
+ struct eeprom_cmd cmd;
+
+ printf_P(PSTR("init commands:\n"));
+ max = eeprom_get_ncmds();
+ for (i = 0; i < max; i++) {
+ eeprom_get_cmd(&cmd, i);
+ printf_P(PSTR("%.2d: %s"), i, cmd.buf);
+ }
+}
+
+int8_t eeprom_insert_cmd_before(const char *str, uint8_t n)
+{
+ uint8_t i, max;
+ struct eeprom_cmd cmd;
+
+ if (strlen(str) >= EEPROM_CMD_SIZE)
+ return -1;
+
+ max = eeprom_get_ncmds();
+ if (n > max)
+ return -1;
+ if (max >= EEPROM_N_CMD_MAX)
+ return -1;
+
+ for (i = max; i > n; i--) {
+ eeprom_get_cmd(&cmd, i-1);
+ eeprom_set_cmd(&cmd, i);
+ }
+
+ snprintf(cmd.buf, sizeof(cmd.buf), "%s", str);
+ eeprom_set_cmd(&cmd, n);
+ eeprom_set_ncmds(max + 1);
+ return 0;
+}
+
+int8_t eeprom_append_cmd(const char *str)
+{
+ uint8_t max;
+
+ max = eeprom_get_ncmds();
+ return eeprom_insert_cmd_before(str, max);
+}
+
+int8_t eeprom_delete_cmd(uint8_t n)
+{
+ uint8_t i, max;
+ struct eeprom_cmd cmd;
+
+ max = eeprom_get_ncmds();
+ if (n >= max)
+ return -1;
+
+ for (i = n; i < max-1; i++) {
+ eeprom_get_cmd(&cmd, i+1);
+ eeprom_set_cmd(&cmd, i);
+ }
+
+ eeprom_set_ncmds(max - 1);
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright 2013 Olivier Matz <zer0@droids-corp.org>
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+#ifndef _EEPROM_CONFIG_H_
+#define _EEPROM_CONFIG_H_
+
+#define EEPROM_CONFIG_MAGIC 0x666bea57
+#define EEPROM_CMD_SIZE 64
+#define EEPROM_N_CMD_MAX 16
+
+struct eeprom_cmd {
+ char buf[EEPROM_CMD_SIZE];
+};
+
+struct eeprom_config
+{
+ uint32_t magic;
+ uint8_t ncmds;
+ struct eeprom_cmd cmds[EEPROM_N_CMD_MAX];
+};
+
+int8_t eeprom_load_config(void);
+uint8_t eeprom_get_ncmds(void);
+void eeprom_set_ncmds(uint8_t ncmds);
+void eeprom_get_cmd(struct eeprom_cmd *cmd, uint8_t n);
+void eeprom_set_cmd(struct eeprom_cmd *cmd, uint8_t n);
+void eeprom_dump_cmds(void);
+int8_t eeprom_insert_cmd_before(const char *str, uint8_t n);
+int8_t eeprom_append_cmd(const char *str);
+int8_t eeprom_delete_cmd(uint8_t n);
+
+#endif
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: error_config.h,v 1.1 2009-02-27 22:23:37 zer0 Exp $
+ *
+ */
+
+#ifndef _ERROR_CONFIG_
+#define _ERROR_CONFIG_
+
+/** enable the dump of the comment */
+#define ERROR_DUMP_TEXTLOG
+
+/** enable the dump of filename and line number */
+//#define ERROR_DUMP_FILE_LINE
+
+#endif
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_config.h,v 1.2 2009-03-05 23:01:32 zer0 Exp $
+ *
+ */
+
+
+#define I2C_BITRATE 1 // divider dor i2c baudrate, see TWBR in doc
+#define I2C_PRESCALER 3 // prescaler config, rate = 2^(n*2)
+
+/* Size of transmission buffer */
+#define I2C_SEND_BUFFER_SIZE 32
+
+/* Size of reception buffer */
+#define I2C_RECV_BUFFER_SIZE 32
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_protocol.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include <aversive/pgmspace.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <callout.h>
+#include <i2c.h>
+
+#include "../common/i2c_commands.h"
+#include "i2c_protocol.h"
+#include "beep.h"
+#include "main.h"
+
+#define I2C_STATE_MAX 2
+#define I2C_PERIOD_MS 50
+
+#define I2C_TIMEOUT 100 /* ms */
+#define I2C_MAX_ERRORS 40
+
+static volatile uint8_t i2c_poll_num = 0;
+static volatile uint8_t i2c_state = 0;
+static volatile uint8_t i2c_rx_count = 0;
+static volatile uint8_t i2c_tx_count = 0;
+static volatile uint16_t i2c_errors = 0;
+
+static uint8_t gps_ok = 0;
+
+#define OP_READY 0 /* no i2c op running */
+#define OP_POLL 1 /* a user command is running */
+#define OP_CMD 2 /* a polling (req / ans) is running */
+
+static volatile uint8_t running_op = OP_READY;
+
+#define I2C_MAX_LOG 3
+static uint8_t error_log = 0;
+
+static struct callout i2c_timer;
+
+static int8_t i2c_req_imuboard_status(void);
+
+/* latest received imuboard_status */
+struct i2c_ans_imuboard_status imuboard_status;
+
+/* used for commands */
+uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
+volatile int8_t command_dest=-1;
+volatile uint8_t command_size=0;
+
+#define I2C_ERROR(args...) do { \
+ if (error_log < I2C_MAX_LOG) { \
+ ERROR(E_USER_I2C_PROTO, args); \
+ error_log ++; \
+ if (error_log == I2C_MAX_LOG) { \
+ ERROR(E_USER_I2C_PROTO, \
+ "i2c logs are now warnings"); \
+ } \
+ } \
+ else \
+ WARNING(E_USER_I2C_PROTO, args); \
+ } while(0)
+
+void i2c_protocol_debug(void)
+{
+ printf_P(PSTR("I2C protocol debug infos:\r\n"));
+ printf_P(PSTR(" i2c_send=%d\r\n"), i2c_tx_count);
+ printf_P(PSTR(" i2c_recv=%d\r\n"), i2c_rx_count);
+ printf_P(PSTR(" i2c_state=%d\r\n"), i2c_state);
+ printf_P(PSTR(" i2c_errors=%d\r\n"), i2c_errors);
+ printf_P(PSTR(" running_op=%d\r\n"), running_op);
+ printf_P(PSTR(" command_size=%d\r\n"), command_size);
+ printf_P(PSTR(" command_dest=%d\r\n"), command_dest);
+ printf_P(PSTR(" i2c_status=%x\r\n"), i2c_status());
+}
+
+static void i2cproto_next_state(uint8_t inc)
+{
+ i2c_state += inc;
+ if (i2c_state >= I2C_STATE_MAX) {
+ i2c_state = 0;
+ i2c_poll_num ++;
+ }
+}
+
+void i2cproto_wait_update(void)
+{
+ uint8_t poll_num;
+ poll_num = i2c_poll_num;
+ (void)poll_num;
+ //WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150); /* XXX todo */
+}
+
+/* called periodically : the goal of this 'thread' is to send requests
+ * and read answers on i2c slaves in the correct order. */
+static void i2c_poll_slaves(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+ uint8_t flags;
+ int8_t err;
+
+ (void)cm;
+ (void)tim;
+ (void)arg;
+
+#if 0
+ static uint8_t a = 0;
+
+ a++;
+ if (a & 0x4)
+ LED2_TOGGLE();
+#endif
+
+ /* already running */
+ IRQ_LOCK(flags);
+ if (running_op != OP_READY) {
+ IRQ_UNLOCK(flags);
+ goto reschedule;
+ }
+
+ /* if a command is ready to be sent, so send it */
+ if (command_size) {
+ running_op = OP_CMD;
+ err = i2c_send(command_dest, command_buf, command_size,
+ I2C_CTRL_GENERIC);
+ if (err < 0)
+ goto error;
+ IRQ_UNLOCK(flags);
+ goto reschedule;
+ }
+
+ /* no command, so do the polling */
+ running_op = OP_POLL;
+
+ switch(i2c_state) {
+
+ /* poll status of imuboard */
+#define I2C_REQ_IMUBOARD 0
+ case I2C_REQ_IMUBOARD:
+ if ((err = i2c_req_imuboard_status()))
+ goto error;
+ break;
+
+#define I2C_ANS_IMUBOARD 1
+ case I2C_ANS_IMUBOARD:
+ if ((err = i2c_recv(I2C_IMUBOARD_ADDR,
+ sizeof(struct i2c_ans_imuboard_status),
+ I2C_CTRL_GENERIC)))
+ goto error;
+ break;
+
+ /* sync with I2C_STATE_MAX */
+
+ /* nothing, go to the first request */
+ default:
+ i2c_state = 0;
+ running_op = OP_READY;
+ }
+ IRQ_UNLOCK(flags);
+
+ goto reschedule;
+
+ error:
+ running_op = OP_READY;
+ IRQ_UNLOCK(flags);
+ i2c_errors++;
+ if (i2c_errors > I2C_MAX_ERRORS) {
+ I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
+ "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
+ i2c_reset();
+ i2c_errors = 0;
+ }
+
+ reschedule:
+ /* reschedule */
+ callout_reschedule(cm, tim, I2C_PERIOD_MS);
+}
+
+/* called when the xmit is finished */
+void i2c_sendevent(int8_t size)
+{
+ if (size > 0) {
+ if (running_op == OP_POLL) {
+ i2cproto_next_state(1);
+ }
+ else
+ command_size = 0;
+ i2c_tx_count++;
+ }
+ else {
+ i2c_errors++;
+ NOTICE(E_USER_I2C_PROTO, "send error state=%d size=%d "
+ "op=%d", i2c_state, size, running_op);
+ if (i2c_errors > I2C_MAX_ERRORS) {
+ I2C_ERROR("I2C error, slave not ready");
+ i2c_reset();
+ i2c_errors = 0;
+ }
+
+ if (running_op == OP_POLL) {
+ /* skip associated answer */
+ i2cproto_next_state(2);
+ }
+ }
+ running_op = OP_READY;
+}
+
+/* called rx event */
+void i2c_recvevent(uint8_t * buf, int8_t size)
+{
+ if (running_op == OP_POLL)
+ i2cproto_next_state(1);
+
+ /* recv is only trigged after a poll */
+ running_op = OP_READY;
+
+ if (size < 0) {
+ goto error;
+ }
+
+ i2c_rx_count++;
+
+ switch (buf[0]) {
+
+ case I2C_ANS_IMUBOARD_STATUS: {
+ struct i2c_ans_imuboard_status *ans =
+ (struct i2c_ans_imuboard_status *)buf;
+
+ if (size != sizeof (*ans))
+ goto error;
+
+ /* copy status in a global struct */
+ memcpy(&imuboard_status, ans, sizeof(imuboard_status));
+
+ if (gps_ok == 0 &&
+ (imuboard_status.flags & IMUBOARD_STATUS_GPS_OK)) {
+ gps_ok = 1;
+ beep(0, 1, 1);
+ beep(0, 1, 1);
+ }
+
+ break;
+ }
+
+ default:
+ break;
+ }
+
+ return;
+ error:
+ i2c_errors++;
+ NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
+ i2c_state, running_op);
+ if (i2c_errors > I2C_MAX_ERRORS) {
+ I2C_ERROR("I2C error, slave not ready");
+ i2c_reset();
+ i2c_errors = 0;
+ }
+}
+
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
+{
+ (void)hwstatus;
+ (void)i;
+ (void)c;
+}
+
+/* ******** ******** ******** ******** */
+/* commands */
+/* ******** ******** ******** ******** */
+
+
+static int8_t
+i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
+{
+ uint8_t flags;
+ uint16_t ms = get_time_ms();
+
+ while ((get_time_ms() - ms) < I2C_TIMEOUT) {
+ IRQ_LOCK(flags);
+ if (command_size == 0) {
+ memcpy(command_buf, buf, size);
+ command_size = size;
+ command_dest = addr;
+ IRQ_UNLOCK(flags);
+ return 0;
+ }
+ IRQ_UNLOCK(flags);
+ }
+ /* this should not happen... except if we are called from an
+ * interrupt context, but it's forbidden */
+ I2C_ERROR("I2C command send failed");
+ return -EBUSY;
+}
+
+static int8_t i2c_req_imuboard_status(void)
+{
+ struct i2c_req_imuboard_status buf;
+ int8_t err;
+
+ buf.hdr.cmd = I2C_REQ_IMUBOARD_STATUS;
+ err = i2c_send(I2C_IMUBOARD_ADDR, (uint8_t*)&buf,
+ sizeof(buf), I2C_CTRL_GENERIC);
+
+ return err;
+}
+
+int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
+{
+ struct i2c_cmd_led_control buf;
+ buf.hdr.cmd = I2C_CMD_GENERIC_LED_CONTROL;
+ buf.led_num = led;
+ buf.state = state;
+ return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
+}
+
+void i2c_protocol_init(void)
+{
+ callout_init(&i2c_timer, i2c_poll_slaves, NULL, I2C_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &i2c_timer, I2C_PERIOD_MS);
+}
--- /dev/null
+/*
+ * Copyright Droids Corporation (2009)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: i2c_protocol.h,v 1.6 2009-11-08 17:24:33 zer0 Exp $
+ *
+ */
+
+#ifndef _I2C_PROTOCOL_H_
+#define _I2C_PROTOCOL_H_
+
+/* load the timer for i2c protocol */
+void i2c_protocol_init(void);
+
+/* dump i2c debug infos (stats) */
+void i2c_protocol_debug(void);
+
+/* wait that all received status have been updated once */
+void i2cproto_wait_update(void);
+
+void i2c_recvevent(uint8_t *buf, int8_t size);
+void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c);
+void i2c_sendevent(int8_t size);
+
+/* control the led of another board (debug) */
+int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state);
+
+/* latest received imuboard_status. Access with care as it can be modified in
+ * the i2c timer. */
+struct i2c_ans_imuboard_status imuboard_status;
+
+#endif
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* fuses:
+ * avrdude -p atmega1284p -P usb -c avrispmkii -U lfuse:w:0xff:m -U hfuse:w:0x91:m -U efuse:w:0xff:m
+ * -> it failed but I answered y, then make reset and it was ok
+ */
+
+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/endian.h>
+#include <aversive/wait.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+
+#include <stdio.h>
+#include <string.h>
+#include <stdint.h>
+#include <inttypes.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <errno.h>
+#include <ctype.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <timer.h>
+#include <i2c.h>
+
+#include "../common/i2c_commands.h"
+#include "eeprom_config.h"
+#include "beep.h"
+#include "xbee_user.h"
+#include "i2c_protocol.h"
+#include "main.h"
+
+struct xbeeboard xbeeboard;
+volatile uint32_t global_ms;
+
+/* global xbee device */
+struct xbee_dev *xbee_dev;
+
+void bootloader(void)
+{
+#define BOOTLOADER_ADDR 0x3f000
+ if (pgm_read_byte_far(BOOTLOADER_ADDR) == 0xff) {
+ printf_P(PSTR("Bootloader is not present\r\n"));
+ return;
+ }
+ cli();
+ /* ... very specific :( */
+ TIMSK0 = 0;
+ TIMSK1 = 0;
+ TIMSK2 = 0;
+ TIMSK3 = 0;
+ EIMSK = 0;
+ UCSR0B = 0;
+ UCSR1B = 0;
+ SPCR = 0;
+ TWCR = 0;
+ ACSR = 0;
+ ADCSRA = 0;
+
+ /* XXX */
+ /* __asm__ __volatile__ ("ldi r31,0xf8\n"); */
+ /* __asm__ __volatile__ ("ldi r30,0x00\n"); */
+ /* __asm__ __volatile__ ("eijmp\n"); */
+}
+
+/* return time in milliseconds on unsigned 16 bits */
+uint16_t get_time_ms(void)
+{
+ uint16_t ms;
+ uint8_t flags;
+ IRQ_LOCK(flags);
+ ms = global_ms;
+ IRQ_UNLOCK(flags);
+ return ms;
+}
+
+static void main_timer_interrupt(void)
+{
+ static uint16_t cycles;
+ static uint8_t cpt = 0;
+ static uint8_t stack = 0;
+
+ cpt++;
+
+ /* LED blink */
+ if (global_ms & 0x80)
+ LED1_ON();
+ else
+ LED1_OFF();
+
+ if (cpt & beep_mask)
+ BUZZER_ON();
+ else
+ BUZZER_OFF();
+
+ if ((cpt & 0x03) != 0)
+ return;
+
+ /* the following code is only called one interrupt among 4: every 682us
+ * (at 12 Mhz) = 8192 cycles */
+ cycles += 8192;
+ if (cycles >= 12000) {
+ cycles -= 12000;
+ global_ms ++;
+ }
+
+ /* called */
+ if (stack++ == 0)
+ LED2_ON();
+ sei();
+ if ((cpt & 0x3) == 0)
+ callout_manage(&xbeeboard.intr_cm);
+ cli();
+ if (--stack == 0)
+ LED2_OFF();
+}
+
+int main(void)
+{
+ FILE *xbee_file;
+ int8_t err;
+ struct xbee_dev dev;
+
+ DDRA = 0x07 /* LEDs */ | 0x10 /* buzzer */;
+
+ uart_init();
+ uart_register_rx_event(CMDLINE_UART, emergency);
+
+ fdevopen(cmdline_dev_send, cmdline_dev_recv);
+ xbee_file = fdevopen(xbee_dev_send, xbee_dev_recv);
+ timer_init();
+ timer0_register_OV_intr(main_timer_interrupt);
+
+ callout_mgr_init(&xbeeboard.intr_cm, get_time_ms);
+
+ cmdline_init();
+ /* LOGS */
+ error_register_emerg(mylog);
+ error_register_error(mylog);
+ error_register_warning(mylog);
+ error_register_notice(mylog);
+ error_register_debug(mylog);
+
+ /* I2C */
+ i2c_init(I2C_MODE_MASTER, I2C_MAINBOARD_ADDR);
+ i2c_protocol_init();
+ i2c_register_recv_event(i2c_recvevent);
+ i2c_register_send_event(i2c_sendevent);
+
+ spi_servo_init();
+ beep_init();
+
+ /* initialize libxbee */
+ err = xbee_init();
+ if (err < 0)
+ return -1;
+
+ xbee_dev = &dev;
+
+ /* open xbee device */
+ if (xbee_open(xbee_dev, xbee_file) < 0)
+ return -1;
+
+ /* register default channel with a callback */
+ if (xbee_register_channel(xbee_dev, XBEE_DEFAULT_CHANNEL,
+ xbeeapp_rx, NULL) < 0) {
+ fprintf(stderr, "cannot register default channel\n");
+ return -1;
+ }
+
+ xbeeapp_init();
+
+ rc_proto_init();
+
+ sei();
+
+ eeprom_load_config();
+
+ printf_P(PSTR("\r\n"));
+ rdline_newline(&xbeeboard.rdl, xbeeboard.prompt);
+
+ xbee_mainloop();
+ return 0;
+}
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/pgmspace.h>
+#include <aversive/error.h>
+
+#include <uart.h>
+#include <parse.h>
+#include <rdline.h>
+#include <timer.h>
+#include <xbee.h>
+#include <callout.h>
+
+#include "cmdline.h"
+#include "rc_proto.h"
+#include "spi_servo.h"
+
+#define NB_LOGS 4
+
+/** ERROR NUMS */
+#define E_USER_DEFAULT 194
+#define E_USER_XBEE 195
+#define E_USER_RC_PROTO 196
+#define E_USER_I2C_PROTO 197
+
+#define LED1_ON() sbi(PORTA, 2)
+#define LED1_OFF() cbi(PORTA, 2)
+
+#define LED2_ON() sbi(PORTA, 1)
+#define LED2_OFF() cbi(PORTA, 1)
+
+#define LED3_ON() sbi(PORTA, 0)
+#define LED3_OFF() cbi(PORTA, 0)
+
+#define BUZZER_ON() sbi(PORTA, 4)
+#define BUZZER_OFF() cbi(PORTA, 4)
+
+/* highest priority */
+#define LED_PRIO 160
+#define TIME_PRIO 140
+#define BEEP_PRIO 120
+#define SPI_PRIO 100 /* users of spi_servo must have lower prio */
+#define XBEE_PRIO 80
+#define I2C_PRIO 70
+#define LOW_PRIO 60
+/* lowest priority */
+
+#define MAX_POWER_LEVEL 5
+/* generic to all boards */
+struct xbeeboard {
+ /* command line interface */
+ struct rdline rdl;
+ char prompt[RDLINE_PROMPT_SIZE];
+
+ struct callout_mgr intr_cm;
+
+ /* log */
+ uint8_t logs[NB_LOGS+1];
+ uint8_t log_level;
+ uint8_t debug;
+};
+extern struct xbeeboard xbeeboard;
+
+extern volatile uint32_t global_ms;
+
+void bootloader(void);
+uint16_t get_time_ms(void);
--- /dev/null
+dump xbee stats
+===============
+
+read rf-errors
+read good-packets
+read tx-errors
+read tx-ack-errors # does not work
+read rx-signal-strength
+read duty-cycle
+read rssi-for-channel # does not work
+
+configure xbee module
+=====================
+
+write bcast-multi-xmit 0
+write unicast-retries 0
+write power-level 0
+
+radio protocol
+==============
+
+- tout en network order
+- avoir le paquet le plus petit possible
+- grouper les informations en un paquet si possible. certaines infos
+ non critiques pourraient attendre en file qu'un paquet de même
+ puissance sorte
+
+Octet 1: type
+
+- si type est < 0x3f, alors il s'agit du bitmask des servos qui seront
+ présents dans le corps. Les servos sont présents par blocs de 10bits.
+ la puissance d'émission est également embarquée sur 3 bits.
+ Cette commande doit être bien compacte car c'est celle qui va être
+ envoyée le plus souvent.
+
+ Exemple, on veut envoyer le servo 0 (=0x123) et 4 (=0x234). Puissance = 2.
+
+ type = 0x5
+ seq_num sur 5 bits
+ puis pow sur 3 bits
+ puis val[0] sur 10 bits
+ puis val[2] sur 10 bits
+ padding à 0 pour arondir sur un octet
+
+ -> 0x11 0x9 0x1c 0x68
+
+- sinon, type correspond à la commande qui est mappée sur une structure
+
+rc_command
+----------
+
+- on récupère l'état du PPM provenant de la télécommande en tache de
+ fond (période 20ms pour chaque servo, et les servos arrivent les
+ uns après les autres)
+
+- toutes les 1 à 5ms
+
+ - on compare les valeurs ppm aux dernières valeurs envoyées: si
+ elles sont différentes:
+
+ - on les stoque en mémoire dans "valeurs à envoyer"
+ - on incrémente un numéro de séquence de cette structure sauf si
+ le dernier numéro d'acquitement reçu est trop ancien
+
+ - si le numéro de séquence est différent du numéro de seq reçu,
+ et que le dernier paquet a été émis il y a plus de 30ms, on émet
+ les nouvelles valeurs et le numéro de séquence local
+
+- lorsqu'on reçoit un paquet d'acquitement, on stoque le numéro de
+ séquence
+
+- lorsqu'on reçoit un paquet de stats, on le traite (affichage, beep,
+ log, osd)
+
+- lorsqu'on reçoit un paquet de "ping", on note le RSSI et la puissance
+ à laquelle il a été émis.
+
+- la puissance d'émission est choisi avec l'algorithme suivant:
+
+ - si on n'a pas reçu d'acquitement ou de ping depuis 500ms, alors
+ on émet alternativement à 0 et 4.
+
+ - sinon, on émet à la puissance la plus faible qui a un RSSI supérieur
+ à un seuil, ce que l'on peut déterminer grâce au ping.
+
+wing
+----
+
+- lorsqu'on reçoit un paquet de commande
+
+ - on met à jour les valeurs des servos
+ - si le dernier paquet d'acquitement a été émis il y a plus de 200ms
+ on émet un paquet d'acquitement. La puissance d'émission du paquet
+ d'acquitement est la même que la puissance du paquet reçu.
+
+- toutes les 500ms, on émet un paquet de "ping" à une puissance
+ différente (0 à 4). La puissance est contenue dans le message.
+
+- toutes les 100ms, on émet un paquet de stats à la puissance du dernier
+ paquet reçu.
+
+interrupt, software interrupts and background tasks
+===================================================
+
+problem description
+-------------------
+
+Today, both command line code, xbee char reception and callout events
+are handled in the main loop, with the lowest priority. Therefore, the
+command line cannot block, else the timers + xbee rx won't be executed.
+
+A typical example where the command line blocks is the completion with
+many choices.
+
+solution
+--------
+
+xbee rx and timers should be executed in a low prio scheduler
+event. With this modification, the command line can block. However we
+must prevent a xbee transmission to be interrupted by a task doing
+another xbee transmission. For that, we will use scheduler_set_prio().
+
+The problem with that is that we can loose precision for low prio
+events if the scheduler interrupt occurs when the scheduler priority
+is low. It could be fixed in scheduler, but maybe we don't need it.
+
+Priorities
+----------
+
+From highest to lowest priority.
+
+- Interrupts:
+
+ - timer
+ - uart
+ - spi
+ - i2c
+
+- Events, called from timer interrupt after a call to sei():
+
+ - LED_PRIO: led blink
+ - BEEP_PRIO: process beep requests and modifies the beep_mask
+ - SPI_PRIO: send and receive SPI data to the atmega168p
+ - CS_PRIO: do the control system of the wing, reading infos from a
+ structure updated by i2c, and writing commands using
+ spi_set_servo()
+ - XBEE_PRIO: read pending chars on xbee serial line and process xbee commands,
+ send xbee commands.
+ - I2C_PRIO: send requests and read answers on i2c slaves
+ - CALLOUT_PRIO: calls low priority events using the callout
+ code. This method allows to execute timers with a fixed period
+ without drift. Even if the event cannot be executed during some
+ time, periodical events will keep the proper period.
+
+- Main loop:
+
+ - command line
+
+Rules
+-----
+
+Calling a xbee function needs to set prio to XBEE_PRIO only.
+Calling i2c_send_command must be done from a lower prio than I2C_PRIO.
+
+libxbee / xbee module
+=====================
+
+Library
+-------
+
+xbee_dev = structure describing an xbee device
+xbee_channel = structure describing a channel, used to associates a req
+ with its answer
+
+xbee.[ch]: main interface to libxbee, creation of dev, registeration of channels
+xbee_atcmd.[ch]: get an AT command from its name or small id string
+xbee_buf.[ch]: not used
+xbee_neighbor.[ch]: helper to add/delete neighbors, use malloc()/free()
+xbee_rxtx.[ch]: parse rx frames, provides xbee_proto_xmit()
+xbee_stats.[ch]: helper to maintain stats
+
+App
+---
+
+move this in library ?
+
+xbeeapp_send(addr, data, len, timeout, cb, arg)
+XXX to detail
+
+Timeout for xbee commands
+=========================
+
+- send_atcmd or send_msg (xbee_prio)
+
+ - allocate a channel and a context
+ - fill the context with info
+ - send the message
+ - load a timeout (xbee_prio)
+
+- on timeout (xbee_prio)
+
+ - log error
+ - call the cb with retcode == TIMEOUT
+ - unregister channel
+ - remove timer
+ - this could be a critical error
+ - if it's a comm error, it's ok
+ - but if the channel is registered again and 1st callback finally occurs...
+
+- on rx (xbee_prio)
+
+ - if there is a context, it's related to a query:
+ - remove the timeout
+ - unregister channel
+ - do basic checks on the frame
+ - log (hexdump)
+ - the frame must be checked in the cb, we can provide helpers
+
+Tests to do
+===========
+
+::
+
+ Radio Controller ))) xbee ((( WING
+ /
+ /
+ PC with command line
+ (through serial)
+
+Test 1: send data, unidirectional
+---------------------------------
+
+Periodically send data from RC to WING (every 20 to 100 ms). The WING sends its
+statistics every second.
+
+Variables:
+
+- period: from 20ms to 200ms
+- packet size: from 1 byte to 20 bytes
+- total number of packets: 1K for short tests, 100K for robustness
+
+If the test is long, take care about maximum duty cycles (10%).
+
+Output:
+
+- local and peer statistics for each test (packet size, pps, number of packets)
+- at the end we will know what is the highest speed we can support for a given
+ packet size, and how reliable is the xbee
+
+Test 2: send data, bidirectional
+--------------------------------
+
+Same than above, except that the WING replies to each received packet with a
+packet of same size.
+
+Same kind of output, ans also measure latency.
+
+Test 3: test wing control on ground
+-----------------------------------
+
+On the ground, try to control the wing with the RC board. We also use the 2.4
+Ghz controller as a fallback (bypass mode).
+
+Check that the bypass mode is enabled when we ask it (for instance on the
+channnel 5 switch) or when we switch off the RC board.
+
+Test 4: embed in WING and send hello
+------------------------------------
+
+The WING board is embeded during a flight. The RC board sends hello message at a
+specified rate (choosen from results of test 1 and 2). The WING sends
+statistics.
+
+Check how the wing distance impacts:
+- the statistics
+- the RX DB
+
+Maybe check with and without the 433Mhz beacon.
+
+Test 5: embed in WING and send dummy servo commands
+---------------------------------------------------
+
+Test in real conditions: the WING board is embeded during a flight. The RC board
+sends dummy servo values at a specified rate (choosen from results of test 1 and
+2). The WING sends statistics and "power probes" allowing the RC board to choose
+its tx power level.
+
+Check how the wing distance impacts:
+- the tx power level of the RC board
+- the statistics
+- the RX DB
+
+Maybe check with and without the 433Mhz beacon.
+
+Output:
+
+After this test, we should be confident that xbee is useable in real conditions.
+
+Test 6: control the wing with the RC board
+------------------------------------------
+
+Same than test 3, but in real flight conditions.
+
+
+-----------------------
+
+test report
+===========
+
+test1
+=====
+
+no stats sent from wing (except if contrary specified)
+don't forget to run read duty-cycle
+to reset duty-cycle, use "write soft-reset"
+
+Be careful, some packets are dropped even with power level = 0 due to over
+power. Need to use a faraday cage.
+
+50ms
+-------
+
+1 byte per msg
+20B/s (160b/s) of useful bandwidth
+mainboard > rc_proto_hello wing 50 1000 x
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+each test (50s) eats ~5% of duty cycle
+
+10 bytes per msg
+200B/s (1.6Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 50 1000 0123456789
+with stats send every second:
+ 950/1000
+ 994/1000
+ 950/1000
+ 1000/1000
+ 999/1000
+without stats:
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+20 bytes per msg
+400B/s (3.2Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 50 1000 01234567890123456789
+ 1000/1000
+ 1000/1000
+
+40 ms
+-------
+
+1 byte per msg
+25B/s (200b/s) of useful bandwidth
+mainboard > rc_proto_hello wing 40 1000 x
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+10 bytes per msg
+250B/s (2Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 40 1000 0123456789
+ 998/1000
+ 1000/1000
+ 1000/1000
+
+20 bytes per msg
+500B/s (4Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 40 1000 01234567890123456789
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+30 ms
+-------
+
+1 byte per msg
+33B/s (300b/s) of useful bandwidth
+mainboard > rc_proto_hello wing 30 1000 x
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+10 bytes per msg
+333B/s (3Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 30 1000 0123456789
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+20 bytes per msg
+666B/s (6Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 30 1000 01234567890123456789
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+20 ms
+-------
+
+1 byte per msg
+50B/s (400b/s) of useful bandwidth
+mainboard > rc_proto_hello wing 20 1000 x
+ 991/1000
+ 991/1000
+ 991/1000
+
+10 bytes per msg
+500B/s (4Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 20 1000 0123456789
+ 863/1000
+ 864/1000
+ 864/1000
+
+20 bytes per msg
+1000B/s (8Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 20 1000 01234567890123456789
+ 763/1000
+ 763/1000
+ 763/1000
+
+25 ms
+-------
+
+1 byte per msg
+40B/s (320b/s) of useful bandwidth
+mainboard > rc_proto_hello wing 25 1000 x
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+10 bytes per msg
+400B/s (3.2Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 25 1000 0123456789
+ 1000/1000
+ 1000/1000
+ 1000/1000
+
+20 bytes per msg
+800B/s (6.4Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 25 1000 01234567890123456789
+ 950/1000
+ 950/1000
+ 950/1000
+
+
+With stats
+===============
+
+25 ms
+-------
+
+1 byte per msg
+40B/s (320b/s) of useful bandwidth
+mainboard > rc_proto_hello wing 25 1000 x
+ 1000/1000
+ 952/1000
+ 1000/1000
+
+10 bytes per msg
+400B/s (3.2Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 25 1000 0123456789
+ 966/1000
+ 974/1000
+ 964/1000
+
+20 bytes per msg
+800B/s (6.4Kb/s) of useful bandwidth
+mainboard > rc_proto_hello wing 25 1000 01234567890123456789
+ 898/1000
+ 898/1000
+
+
+Latency
+===============
+
+the ECHO command contains a timestamp, allowing latency measurement.
+the structure is a but larger than a HELLO message.
+
+mainboard > rc_proto_stats reset
+mainboard > rc_proto_echo wing 50 50 x
+mainboard > rc_proto_stats show
+ echo_ans_latency_ms: 80
+
+mainboard > rc_proto_stats reset
+mainboard > rc_proto_echo wing 50 50 0123456789
+mainboard > rc_proto_stats show
+ echo_ans_latency_ms: 95
+
+mainboard > rc_proto_stats reset
+mainboard > rc_proto_echo wing 50 50 01234567890123456789
+mainboard > rc_proto_stats show
+ echo_ans_latency_ms: 125
+
+The latency is probably small enough for our RC application, but it is
+not as small as expected.
+
+serial from uc to xbee at 57600 = ~3ms for 20 bytes
+radio rate is 24000: ~6.66ms for 20 bytes
+
+the progression of latency is not linear.
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <aversive/pgmspace.h>
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <ctype.h>
+#include <string.h>
+
+#include <parse.h>
+#include <xbee.h>
+
+#include "parse_atcmd.h"
+
+static int8_t
+parse_atcmd(PGM_P tk, const char *buf, void *res)
+{
+ struct xbee_atcmd copy;
+ struct token_atcmd_data ad;
+ const struct xbee_atcmd *cmd;
+ char bufcopy[32];
+ uint8_t token_len = 0;
+
+ memcpy_P(&ad, &((struct token_atcmd *)tk)->atcmd_data, sizeof(ad));
+
+ while (!isendoftoken(buf[token_len]) &&
+ token_len < (sizeof(bufcopy)-1)) {
+ bufcopy[token_len] = buf[token_len];
+ token_len++;
+ }
+ bufcopy[token_len] = 0;
+
+ /* XXX should be related to an xbee device */
+ cmd = xbee_atcmd_lookup_desc(bufcopy);
+
+ if (cmd == NULL)
+ return -1;
+
+ /* command must match flags */
+ memcpy_P(©, cmd, sizeof(copy));
+ if ((copy.flags & ad.atcmd_mask) != ad.atcmd_flags)
+ return -1;
+ /* store the address of object in structure */
+ if (res)
+ *(const struct xbee_atcmd **)res = cmd;
+
+ return token_len;
+}
+
+static int8_t complete_get_nb_atcmd(PGM_P tk)
+{
+ struct token_atcmd_data ad;
+ const struct xbee_atcmd *cmd;
+ struct xbee_atcmd copy;
+ int8_t cnt = 0;
+
+ memcpy_P(&ad, &((struct token_atcmd *)tk)->atcmd_data, sizeof(ad));
+
+ for (cmd = &xbee_atcmd_list[0], memcpy_P(©, cmd, sizeof(copy));
+ copy.name != NULL;
+ cmd++, memcpy_P(©, cmd, sizeof(copy))) {
+
+ if ((copy.flags & ad.atcmd_mask) == ad.atcmd_flags)
+ cnt++;
+ }
+ return cnt;
+}
+
+static int8_t complete_get_elt_atcmd(PGM_P tk, int8_t idx,
+ char *dstbuf, uint8_t size)
+{
+ struct token_atcmd_data ad;
+ const struct xbee_atcmd *cmd;
+ struct xbee_atcmd copy;
+ int8_t cnt = 0;
+
+ memcpy_P(&ad, &((struct token_atcmd *)tk)->atcmd_data, sizeof(ad));
+
+ for (cmd = &xbee_atcmd_list[0], memcpy_P(©, cmd, sizeof(copy));
+ copy.name != NULL;
+ cmd++, memcpy_P(©, cmd, sizeof(copy))) {
+
+ if ((copy.flags & ad.atcmd_mask) == ad.atcmd_flags) {
+ if (cnt == idx) {
+ memcpy_P(dstbuf, copy.desc, size);
+ dstbuf[size-1] = '\0';
+
+ return 0;
+ }
+ cnt++;
+ }
+ }
+ return -1;
+}
+
+static int8_t
+help_atcmd(PGM_P tk, char *dstbuf, uint8_t size)
+{
+ (void)tk;
+ snprintf(dstbuf, size, "ATCMD");
+ return 0;
+}
+
+struct token_ops token_atcmd_ops = {
+ .parse = parse_atcmd,
+ .complete_get_nb = complete_get_nb_atcmd,
+ .complete_get_elt = complete_get_elt_atcmd,
+ .get_help = help_atcmd,
+};
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _PARSE_ATCMD_H_
+#define _PARSE_ATCMD_H_
+
+struct token_atcmd_data {
+ struct xbee_dev **xbee_dev;
+ unsigned atcmd_flags;
+ unsigned atcmd_mask;
+};
+
+struct token_atcmd {
+ struct token_hdr hdr;
+ struct token_atcmd_data atcmd_data;
+};
+typedef struct token_atcmd parse_token_atcmd_t;
+
+struct token_atcmd_pgm {
+ struct token_hdr hdr;
+ struct token_atcmd_data atcmd_data;
+} PROGMEM;
+typedef struct token_atcmd_pgm parse_pgm_token_atcmd_t;
+
+extern struct token_ops token_atcmd_ops;
+
+#define TOKEN_ATCMD_INITIALIZER(structure, field, dev, flags, mask) \
+{ \
+ .hdr = { \
+ .ops = &token_atcmd_ops, \
+ .offset = offsetof(structure, field), \
+ }, \
+ .atcmd_data = { \
+ .xbee_dev = dev, \
+ .atcmd_flags = flags, \
+ .atcmd_mask = mask, \
+ }, \
+}
+
+#endif /* _PARSE_ATCMD_H_ */
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/pgmspace.h>
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <ctype.h>
+#include <string.h>
+
+#include <parse.h>
+
+#include "parse_monitor.h"
+
+struct monitor_reg_list xbee_monitor_list = LIST_HEAD_INITIALIZER();
+
+static int8_t
+parse_monitor(PGM_P tk, const char *buf, void *res)
+{
+ struct monitor_reg *m;
+ uint8_t token_len = 0;
+ char bufcopy[32];
+
+ (void)tk;
+ while (!isendoftoken(buf[token_len]) &&
+ token_len < (sizeof(bufcopy)-1)) {
+ bufcopy[token_len] = buf[token_len];
+ token_len++;
+ }
+ bufcopy[token_len] = 0;
+
+ LIST_FOREACH(m, &xbee_monitor_list, next) {
+ if (!strcmp_P(bufcopy, m->desc))
+ break;
+ }
+ if (m == NULL) /* not found */
+ return -1;
+
+ /* store the address of object in structure */
+ if (res)
+ *(struct monitor_reg **)res = m;
+
+ return token_len;
+}
+
+static int8_t
+complete_get_nb_monitor(PGM_P tk)
+{
+ struct monitor_reg *m;
+ int8_t i = 0;
+
+ (void)tk;
+ LIST_FOREACH(m, &xbee_monitor_list, next) {
+ i++;
+ }
+ return i;
+}
+
+static int8_t
+complete_get_elt_monitor(PGM_P tk, int8_t idx,
+ char *dstbuf, uint8_t size)
+{
+ struct monitor_reg *m;
+ int8_t i = 0, len;
+
+ (void)tk;
+ LIST_FOREACH(m, &xbee_monitor_list, next) {
+ if (i == idx)
+ break;
+ i++;
+ }
+ if (m == NULL)
+ return -1;
+
+ len = snprintf_P(dstbuf, size, PSTR("%S"), m->desc);
+ if (len < 0 || len >= size)
+ return -1;
+
+ return 0;
+}
+
+
+static int8_t
+help_monitor(PGM_P tk, char *dstbuf,
+ uint8_t size)
+{
+ (void)tk;
+ snprintf_P(dstbuf, size, PSTR("Monitor-register"));
+ return 0;
+}
+
+struct token_ops token_monitor_ops = {
+ .parse = parse_monitor,
+ .complete_get_nb = complete_get_nb_monitor,
+ .complete_get_elt = complete_get_elt_monitor,
+ .get_help = help_monitor,
+};
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _PARSE_MONITOR_H_
+#define _PARSE_MONITOR_H_
+
+#include <parse.h>
+
+struct monitor_reg {
+ LIST_ENTRY(monitor_reg) next;
+ const char *desc;
+ char atcmd[3];
+};
+
+LIST_HEAD(monitor_reg_list, monitor_reg);
+extern struct monitor_reg_list xbee_monitor_list;
+
+struct token_monitor_data {
+};
+
+struct token_monitor {
+ struct token_hdr hdr;
+ struct token_monitor_data monitor_data;
+};
+typedef struct token_monitor parse_token_monitor_t;
+
+extern struct token_ops token_monitor_ops;
+
+#define TOKEN_MONITOR_INITIALIZER(structure, field){ \
+ .hdr = { \
+ .ops = &token_monitor_ops, \
+ .offset = offsetof(structure, field), \
+ }, \
+ .monitor_data = { \
+ }, \
+}
+
+#endif /* _PARSE_MONITOR_H_ */
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/pgmspace.h>
+
+#include <stdio.h>
+#include <inttypes.h>
+#include <ctype.h>
+#include <string.h>
+
+#include <parse.h>
+#include <xbee.h>
+
+#include "parse_neighbor.h"
+
+static int8_t
+parse_neighbor(PGM_P tk, const char *buf, void *res)
+{
+ struct token_neighbor_data tkd;
+ struct xbee_dev *dev;
+ struct xbee_neigh *neigh;
+ uint8_t token_len = 0;
+ char bufcopy[32];
+
+ memcpy_P(&tkd, &((struct token_neighbor *)tk)->neighbor_data,
+ sizeof(tkd));
+ dev = *tkd.xbee_dev;
+
+ while (!isendoftoken(buf[token_len]) &&
+ token_len < (sizeof(bufcopy)-1)) {
+ bufcopy[token_len] = buf[token_len];
+ token_len++;
+ }
+ bufcopy[token_len] = 0;
+ neigh = xbee_neigh_lookup(dev, bufcopy);
+ if (neigh == NULL) /* not found */
+ return -1;
+
+ /* store the address of xbee_neigh in structure */
+ if (res)
+ *(struct xbee_neigh **)res = neigh;
+
+ return token_len;
+}
+
+static int8_t
+complete_get_nb_neighbor(PGM_P tk)
+{
+ struct token_neighbor_data tkd;
+ struct xbee_dev *dev;
+ struct xbee_neigh *neigh;
+ int8_t i = 0;
+
+ memcpy_P(&tkd, &((struct token_neighbor *)tk)->neighbor_data,
+ sizeof(tkd));
+ dev = *tkd.xbee_dev;
+
+ LIST_FOREACH(neigh, &dev->neigh_list, next) {
+ i++;
+ }
+ return i;
+}
+
+static int8_t
+complete_get_elt_neighbor(PGM_P tk, int8_t idx,
+ char *dstbuf, uint8_t size)
+{
+ struct token_neighbor_data tkd;
+ struct xbee_dev *dev;
+ struct xbee_neigh *neigh;
+ int8_t i = 0, len;
+
+ memcpy_P(&tkd, &((struct token_neighbor *)tk)->neighbor_data,
+ sizeof(tkd));
+ dev = *tkd.xbee_dev;
+
+ LIST_FOREACH(neigh, &dev->neigh_list, next) {
+ if (i++ == idx)
+ break;
+ }
+
+ if (neigh == NULL)
+ return -1;
+
+ len = snprintf_P(dstbuf, size, PSTR("%s"), neigh->name);
+ if (len < 0 || len >= size)
+ return -1;
+
+ return 0;
+}
+
+
+static int8_t
+help_neighbor(PGM_P tk, char *dstbuf, uint8_t size)
+{
+ (void)tk;
+ snprintf_P(dstbuf, size, PSTR("Neighbor"));
+ return 0;
+}
+
+struct token_ops token_neighbor_ops = {
+ .parse = parse_neighbor,
+ .complete_get_nb = complete_get_nb_neighbor,
+ .complete_get_elt = complete_get_elt_neighbor,
+ .get_help = help_neighbor,
+};
--- /dev/null
+/*
+ * Copyright (c) 2011, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _PARSE_NEIGHBOR_H_
+#define _PARSE_NEIGHBOR_H_
+
+struct token_neighbor_data {
+ struct xbee_dev **xbee_dev;
+};
+
+struct token_neighbor {
+ struct token_hdr hdr;
+ struct token_neighbor_data neighbor_data;
+};
+typedef struct token_neighbor parse_token_neighbor_t;
+
+struct token_neighbor_pgm {
+ struct token_hdr hdr;
+ struct token_neighbor_data neighbor_data;
+} PROGMEM;
+typedef struct token_neighbor_pgm parse_pgm_token_neighbor_t;
+
+extern struct token_ops token_neighbor_ops;
+
+#define TOKEN_NEIGHBOR_INITIALIZER(structure, field, dev) \
+ { \
+ .hdr = { \
+ .ops = &token_neighbor_ops, \
+ .offset = offsetof(structure, field), \
+ }, \
+ .neighbor_data = { \
+ .xbee_dev = dev, \
+ }, \
+}
+
+#endif /* _PARSE_NEIGHBOR_H_ */
--- /dev/null
+/*
+ * Copyright (c) 2013, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <string.h>
+
+#include <aversive.h>
+#include <aversive/queue.h>
+#include <aversive/endian.h>
+
+#include <stdint.h>
+
+#include <uart.h>
+
+#include <parse.h>
+#include <rdline.h>
+#include <timer.h>
+#include <xbee.h>
+
+#include "callout.h"
+#include "rc_proto.h"
+#include "xbee_user.h"
+#include "spi_servo.h"
+#include "main.h"
+
+#define N_SERVO 6
+#define SERVO_NBITS 10
+
+#define RX_DB_THRESHOLD 65 /* mean -65 dB */
+
+/* default values */
+struct rc_proto_timers rc_proto_timers = {
+ .send_servo_min_ms = 50,
+ .send_servo_max_ms = 300,
+ .send_power_probe_ms = 500,
+ .autobypass_ms = 500,
+};
+
+/* rc_proto statistics, accessed with sched_prio=XBEE_PRIO */
+struct rc_proto_stats_data {
+ uint32_t hello_rx;
+ uint32_t hello_tx;
+ uint32_t echo_req_rx;
+ uint32_t echo_req_tx;
+ uint32_t echo_ans_rx;
+ uint32_t echo_ans_tx;
+ uint32_t power_probe_rx;
+ uint32_t power_probe_tx;
+ uint32_t ack_rx;
+ uint32_t ack_tx;
+ uint32_t servo_rx;
+ uint32_t servo_tx;
+ uint32_t stats_rx;
+ uint32_t stats_tx;
+ uint32_t echo_ans_latency_sum;
+};
+static struct rc_proto_stats_data stats; /* local stats */
+static struct rc_proto_stats_data peer_stats; /* peer stats */
+
+/* store last received power probes */
+struct rc_proto_power_levels {
+ uint8_t ttl;
+ uint16_t power_db;
+};
+static struct rc_proto_power_levels power_levels[MAX_POWER_LEVEL];
+
+/* address of the peer */
+static uint64_t rc_proto_dstaddr = 0xFFFF; /* broadcast by default */
+
+/* state sent to the xbee peer (used on RC) */
+struct servo_tx {
+ uint16_t servos[N_SERVO];
+ uint8_t bypass; /* ask the wing to bypass servos = use legacy controller */
+ uint8_t seq; /* from 0 to 15, 4 bits */
+ uint16_t time; /* time of last xmit */
+};
+static struct servo_tx servo_tx;
+
+/* state received from the xbee peer (used on WING) */
+struct servo_rx {
+ uint16_t servos[N_SERVO];
+ uint16_t time; /* time of last xmit */
+};
+static struct servo_rx servo_rx;
+
+/* the received seq value (acknowledged by the wing, received on the rc) */
+static uint8_t ack;
+
+/* define tx mode (disabled, send from spi, bypass), rx mode (auto-bypass),
+ ... */
+static uint8_t rc_proto_flags;
+
+/* callout managing rc_proto (ex: sending of servos periodically) */
+static struct callout rc_proto_timer;
+
+/* a negative value (-1 or -4) means we don't know the best level, but it stores
+ * the previous PL value (0 or 4) so we can alternate. */
+int8_t power_level_global = -1;
+
+/* update power level when we receive the answer from DB. The request is sent by
+ * rc_proto_rx_power_probe(). */
+static int8_t update_power_level(int8_t retcode, void *frame, unsigned len,
+ void *arg)
+{
+ struct xbee_atresp_hdr *atresp = (struct xbee_atresp_hdr *)frame;
+ int level = (intptr_t)arg;
+ uint8_t db;
+
+ /* nothing more to do, error is already logged by xbee_user */
+ if (retcode < 0)
+ return retcode;
+
+ if (len < sizeof(struct xbee_atresp_hdr) + sizeof(uint8_t))
+ return -1;
+
+ db = atresp->data[0];
+ power_levels[level].power_db = db;
+ power_levels[level].ttl = 10; /* valid during 10 seconds */
+ return 0;
+}
+
+/* when we receive a power probe, ask the DB value to the xbee */
+static void rc_proto_rx_power_probe(int power_level)
+{
+ xbeeapp_send_atcmd("DB", NULL, 0, update_power_level,
+ (void *)(intptr_t)power_level);
+}
+
+/* called every second to decide which power should be used */
+static void compute_best_power(void)
+{
+ int8_t best_power_level = -1;
+ int8_t i;
+
+ /* decrement TTL */
+ for (i = 0; i < MAX_POWER_LEVEL; i++) {
+ if (power_levels[i].ttl > 0)
+ power_levels[i].ttl--;
+ }
+
+ for (i = 0; i < MAX_POWER_LEVEL; i++) {
+ if (power_levels[i].ttl == 0)
+ continue;
+
+ /* if signal is powerful enough, select this as level */
+ if (power_levels[i].power_db < RX_DB_THRESHOLD) {
+ best_power_level = i;
+ break;
+ }
+ }
+
+ /* we have no info, don't touch the negative value */
+ if (best_power_level < 0 && power_level_global < 0)
+ return;
+
+ if (power_level_global != best_power_level) {
+ DEBUG(E_USER_RC_PROTO, "changing power level %d => %d\n",
+ power_level_global, best_power_level);
+ }
+ power_level_global = best_power_level;
+}
+
+/* return the best power level, or -1 if best power level computation is
+ * disabled. */
+static int8_t get_best_power(void)
+{
+ if ((rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) == 0)
+ return -1;
+
+ /* special values */
+ if (power_level_global == -1) {
+ power_level_global = -4;
+ return 4;
+ }
+ else if (power_level_global == -4) {
+ power_level_global = -1;
+ return 0;
+ }
+ else
+ return power_level_global;
+}
+
+/* send a hello message: no answer expected */
+int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power)
+{
+ struct rc_proto_hello hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_HELLO;
+ hdr.datalen = data_len;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = data;
+ msg.iov[1].len = data_len;
+
+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.hello_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send an echo message: expect a reply */
+int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power)
+{
+ struct rc_proto_echo_req hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_ECHO_REQ;
+ hdr.power = power;
+ hdr.timestamp = get_time_ms();
+ hdr.datalen = data_len;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = data;
+ msg.iov[1].len = data_len;
+
+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.echo_req_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send an echo message: expect a reply */
+int8_t rc_proto_send_echo_ans(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power, uint16_t timestamp)
+{
+ struct rc_proto_echo_ans hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_ECHO_ANS;
+ hdr.datalen = data_len;
+ hdr.timestamp = timestamp;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = data;
+ msg.iov[1].len = data_len;
+
+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.echo_ans_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send an echo message: expect a reply */
+int8_t rc_proto_send_power_probe(uint64_t addr, uint8_t power)
+{
+ struct rc_proto_power_probe hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_POWER_PROBE;
+ hdr.power_level = power;
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+
+ /* set power level */
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.power_probe_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* convert values from servo_tx.servos into a xbee frame */
+static int8_t servo2buf(uint8_t buf[RC_PROTO_SERVO_LEN],
+ uint8_t seq, uint8_t bypass, uint8_t pow, const uint16_t servos[N_SERVO])
+{
+ uint8_t i = 0;
+
+ buf[i++] = RC_PROTO_SERVO;
+ buf[i++] = ((seq & 0xf) << 4) | (bypass << 3) | (pow & 0x7);
+
+ buf[i++] = servos[0] >> 2;
+ buf[i] = (servos[0] & 0x3) << 6;
+
+ buf[i++] |= servos[1] >> 4;
+ buf[i] = (servos[1] & 0xf) << 4;
+
+ buf[i++] |= servos[2] >> 6;
+ buf[i] = (servos[2] & 0x3f) << 2;
+
+ buf[i++] |= servos[3] >> 8;
+ buf[i++] = servos[3] & 0xff;
+
+ buf[i++] = servos[4] >> 2;
+ buf[i] = (servos[4] & 0x3) << 6;
+
+ buf[i++] |= servos[5] >> 4;
+ buf[i] = (servos[5] & 0xf) << 4;
+
+ return 0;
+}
+
+/* send servos, called periodically with prio = XBEE_PRIO */
+static int8_t rc_proto_send_servos(void)
+{
+ struct rc_proto_servo hdr;
+ struct xbee_msg msg;
+ uint8_t i, updated = 0;
+ uint16_t ms, diff, servo_val;
+ uint8_t frame[RC_PROTO_SERVO_LEN];
+ int8_t ret;
+ uint8_t power;
+
+ /* servo send disabled */
+ if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) == RC_PROTO_FLAGS_TX_OFF)
+ return 0;
+
+ /* if we transmitted servos values recently, nothing to do */
+ ms = get_time_ms();
+ diff = ms - servo_tx.time;
+ if (diff < rc_proto_timers.send_servo_min_ms)
+ return 0;
+
+ /* prepare values to send */
+ if ((rc_proto_flags & RC_PROTO_FLAGS_TX_MASK) ==
+ RC_PROTO_FLAGS_TX_COPY_SPI) {
+
+ /* set bypass to 0 */
+ if (servo_tx.bypass == 1) {
+ servo_tx.bypass = 0;
+ updated = 1;
+ }
+
+ /* copy values from spi */
+ for (i = 0; i < N_SERVO; i++) {
+ servo_val = spi_servo_get(i);
+ if (servo_val != servo_tx.servos[i]) {
+ servo_tx.servos[i] = servo_val;
+ updated = 1;
+ }
+ }
+ }
+ else {
+ /* set bypass to 1 */
+ if (servo_tx.bypass == 0) {
+ servo_tx.bypass = 1;
+ updated = 1;
+ }
+ }
+
+ /* if no value changed and last message is acknowledged, don't transmit
+ * if we already transmitted quite recently */
+ if (updated == 0 && ack == servo_tx.seq &&
+ diff < rc_proto_timers.send_servo_max_ms)
+ return 0;
+
+ /* ok, we need to transmit */
+
+ /* get the new seq value */
+ if (updated == 1) {
+ servo_tx.seq++;
+ servo_tx.seq &= 0x1f;
+ if (servo_tx.seq == ack)
+ servo_tx.seq = (ack - 1) & 0x1f;
+ }
+ /* reset the "updated" flag and save time */
+ servo_tx.time = ms;
+
+ /* set power level */
+ power = get_best_power();
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* create frame and send it */
+ servo2buf(frame, servo_tx.seq, servo_tx.bypass, power, servo_tx.servos);
+ hdr.type = RC_PROTO_SERVO;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = frame;
+ msg.iov[1].len = RC_PROTO_SERVO_LEN;
+
+ stats.servo_tx++;
+ ret = xbeeapp_send_msg(rc_proto_dstaddr, &msg, NULL, NULL);
+ stats.servo_tx++;
+
+ return ret;
+}
+
+/* send a ack message: no answer expected */
+int8_t rc_proto_send_ack(uint64_t addr, uint8_t seq, int8_t power)
+{
+ struct rc_proto_ack hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_ACK;
+ hdr.seq = seq;
+
+ msg.iovlen = 1;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+
+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.ack_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+/* send a hello message: no answer expected */
+int8_t rc_proto_send_stats(uint64_t addr, int8_t power)
+{
+ struct rc_proto_stats hdr;
+ struct xbee_msg msg;
+ uint8_t prio;
+ int8_t ret;
+
+ hdr.type = RC_PROTO_STATS;
+
+ msg.iovlen = 2;
+ msg.iov[0].buf = &hdr;
+ msg.iov[0].len = sizeof(hdr);
+ msg.iov[1].buf = &stats;
+ msg.iov[1].len = sizeof(stats);
+
+ /* set power level */
+ if (power != -1)
+ xbeeapp_send_atcmd("PL", &power, sizeof(power), NULL, NULL);
+
+ /* we need to lock callout to increment stats */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ stats.stats_tx++;
+ ret = xbeeapp_send_msg(addr, &msg, NULL, NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+void rc_proto_set_mode(uint8_t flags)
+{
+ rc_proto_flags = flags;
+}
+
+uint8_t rc_proto_get_mode(void)
+{
+ return rc_proto_flags;
+}
+
+/* convert a receved servo frame into servo values */
+static int8_t buf2servo(uint16_t servos[N_SERVO], const uint8_t *buf)
+{
+ uint16_t val;
+
+ val = buf[1];
+ val <<= 2;
+ val |= (buf[2] >> 6);
+ servos[0] = val;
+
+ val = buf[2] & 0x3f;
+ val <<= 4;
+ val |= (buf[3] >> 4);
+ servos[1] = val;
+
+ val = buf[3] & 0xf;
+ val <<= 6;
+ val |= (buf[4] >> 2);
+ servos[2] = val;
+
+ val = buf[4] & 0x3;
+ val <<= 8;
+ val |= (buf[5]);
+ servos[3] = val;
+
+ val = buf[6];
+ val <<= 2;
+ val |= (buf[7] >> 6);
+ servos[4] = val;
+
+ val = buf[7];
+ val <<= 4;
+ val |= (buf[8] >> 4);
+ servos[5] = val;
+
+ return 0;
+}
+
+/* process a received servo frame */
+static int8_t rc_proto_rx_servo(struct rc_proto_servo *rcs)
+{
+ uint8_t bypass;
+ uint8_t i, seq, pow;
+
+ bypass = !!(rcs->data[0] & 0x08);
+ pow = rcs->data[0] & 0x07;
+
+ /* convert it in a table of servo values */
+ if (bypass == 0 && buf2servo(servo_rx.servos, rcs->data) < 0)
+ return -1;
+
+ /* save time */
+ servo_rx.time = get_time_ms();
+
+ /* acknowledge received frame */
+ seq = rcs->data[0] >> 4;
+ rc_proto_send_ack(rc_proto_dstaddr, seq, pow);
+
+ /* copy values to spi */
+ if (rc_proto_flags & RC_PROTO_FLAGS_RX_COPY_SPI) {
+ spi_servo_set_bypass(bypass);
+
+ if (bypass == 0) {
+ for (i = 0; i < N_SERVO; i++)
+ spi_servo_set(i, servo_rx.servos[i]);
+ }
+ }
+ return 0;
+}
+
+/* receive a rc_proto message */
+int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
+{
+ unsigned int datalen;
+ struct rc_proto_hdr *rch = (struct rc_proto_hdr *) &recvframe->data;
+
+ if (len < sizeof(*recvframe))
+ return -1;
+
+ datalen = len - sizeof(*recvframe);
+ if (datalen < sizeof(struct rc_proto_hdr))
+ return -1;
+
+ /* other command types */
+ switch (rch->type) {
+ case RC_PROTO_HELLO: {
+ struct rc_proto_hello *rch =
+ (struct rc_proto_hello *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv hello len=%d", rch->datalen);
+ stats.hello_rx++;
+ return 0;
+ }
+
+ case RC_PROTO_ECHO_REQ: {
+ struct rc_proto_echo_req *rce =
+ (struct rc_proto_echo_req *) recvframe->data;
+ int8_t power = rce->power;
+
+ NOTICE(E_USER_XBEE, "recv echo len=%d", rce->datalen);
+ stats.echo_req_rx++;
+
+ if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr),
+ rce->data, rce->datalen, power,
+ rce->timestamp) < 0)
+ return -1;
+
+ return 0;
+ }
+
+ case RC_PROTO_ECHO_ANS: {
+ struct rc_proto_echo_ans *rce =
+ (struct rc_proto_echo_ans *) recvframe->data;
+ uint16_t diff;
+
+ NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen);
+ stats.echo_ans_rx++;
+ diff = get_time_ms() - rce->timestamp;
+ stats.echo_ans_latency_sum += diff;
+ return 0;
+ }
+
+ /* received by the radio controller every ~500ms */
+ case RC_PROTO_POWER_PROBE: {
+ struct rc_proto_power_probe *rcpb =
+ (struct rc_proto_power_probe *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv power_probe");
+
+ if (datalen != sizeof(*rcpb))
+ return -1;
+
+ if (rcpb->power_level >= MAX_POWER_LEVEL)
+ return -1;
+
+ stats.power_probe_rx++;
+ /* ask the DB value to the xbee module */
+ rc_proto_rx_power_probe(rcpb->power_level);
+
+ return 0;
+ }
+
+ /* received by the radio controller */
+ case RC_PROTO_ACK: {
+ struct rc_proto_ack *rca =
+ (struct rc_proto_ack *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv ack, ack=%d", rca->seq);
+ stats.ack_rx++;
+ return 0;
+ }
+
+ /* received by the wing */
+ case RC_PROTO_SERVO: {
+ struct rc_proto_servo *rcs =
+ (struct rc_proto_servo *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv servo");
+
+ if (datalen != RC_PROTO_SERVO_LEN)
+ return -1;
+
+ stats.servo_rx++;
+ return rc_proto_rx_servo(rcs);
+ }
+
+ /* received by the radio controller */
+ case RC_PROTO_STATS: {
+ struct rc_proto_stats *rcs =
+ (struct rc_proto_stats *) recvframe->data;
+
+ NOTICE(E_USER_XBEE, "recv stats");
+
+ if (datalen != sizeof(*rcs) + sizeof(peer_stats))
+ return -1;
+
+ stats.stats_rx++;
+ memcpy(&peer_stats, rcs->stats, sizeof(peer_stats));
+ return 0;
+ }
+
+ default:
+ return -1;
+ }
+
+ /* not reached */
+ return 0;
+}
+
+/* called by the scheduler, manage rc_proto periodical tasks */
+static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+ (void)arg;
+ static uint16_t prev_stats_send;
+ static uint16_t prev_compute_pow;
+ static uint16_t prev_power_probe;
+ static uint8_t pow_probe;
+ uint16_t t, diff;
+
+ t = get_time_ms();
+
+ /* send servo values if flags are enabled. The function will decide
+ * by itself if it's time to send or not */
+ rc_proto_send_servos();
+
+ /* send power probe periodically */
+ if (rc_proto_flags & RC_PROTO_FLAGS_TX_POW_PROBE) {
+ diff = t - prev_power_probe;
+ if (diff > rc_proto_timers.send_power_probe_ms) {
+ pow_probe++;
+ if (pow_probe > 4)
+ pow_probe = 0;
+ rc_proto_send_power_probe(rc_proto_dstaddr, pow_probe);
+ prev_power_probe = t;
+ }
+ }
+
+ /* on wing, auto bypass servos if no commands since some time */
+ if (rc_proto_flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) {
+ diff = t - servo_rx.time;
+ if (diff > rc_proto_timers.autobypass_ms)
+ spi_servo_set_bypass(1);
+ }
+
+ /* send stats to peer every second */
+ if (rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) {
+ diff = t - prev_compute_pow;
+ if (diff >= 1000) {
+ compute_best_power();
+ prev_compute_pow = t;
+ }
+ }
+
+ /* send stats to peer every second */
+ if (rc_proto_flags & RC_PROTO_FLAGS_TX_STATS) {
+ diff = t - prev_stats_send;
+ if (diff >= 1000) {
+ rc_proto_send_stats(rc_proto_dstaddr, get_best_power());
+ prev_stats_send = t;
+ }
+ }
+
+ callout_schedule(cm, tim, 0);
+}
+
+void rc_proto_dump_stats(void)
+{
+ printf_P(PSTR("rc_proto stats LOCAL\r\n"));
+ printf_P(PSTR(" hello_tx: %"PRIu32"\r\n"), stats.hello_tx);
+ printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), stats.hello_rx);
+ printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), stats.echo_req_rx);
+ printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), stats.echo_req_tx);
+ printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), stats.echo_ans_rx);
+ printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), stats.echo_ans_tx);
+ printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), stats.power_probe_rx);
+ printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), stats.power_probe_tx);
+ printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), stats.ack_rx);
+ printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), stats.ack_tx);
+ printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), stats.servo_rx);
+ printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), stats.servo_tx);
+ printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), stats.stats_rx);
+ printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), stats.stats_tx);
+ if (stats.echo_ans_rx != 0) {
+ printf_P(PSTR(" echo_ans_latency_ms: %"PRIu32"\r\n"),
+ stats.echo_ans_latency_sum / stats.echo_ans_rx);
+ }
+
+ printf_P(PSTR("rc_proto stats PEER\r\n"));
+ printf_P(PSTR(" hello_tx: %"PRIu32"\r\n"), peer_stats.hello_tx);
+ printf_P(PSTR(" hello_rx: %"PRIu32"\r\n"), peer_stats.hello_rx);
+ printf_P(PSTR(" echo_req_rx: %"PRIu32"\r\n"), peer_stats.echo_req_rx);
+ printf_P(PSTR(" echo_req_tx: %"PRIu32"\r\n"), peer_stats.echo_req_tx);
+ printf_P(PSTR(" echo_ans_rx: %"PRIu32"\r\n"), peer_stats.echo_ans_rx);
+ printf_P(PSTR(" echo_ans_tx: %"PRIu32"\r\n"), peer_stats.echo_ans_tx);
+ printf_P(PSTR(" power_probe_rx: %"PRIu32"\r\n"), peer_stats.power_probe_rx);
+ printf_P(PSTR(" power_probe_tx: %"PRIu32"\r\n"), peer_stats.power_probe_tx);
+ printf_P(PSTR(" ack_rx: %"PRIu32"\r\n"), peer_stats.ack_rx);
+ printf_P(PSTR(" ack_tx: %"PRIu32"\r\n"), peer_stats.ack_tx);
+ printf_P(PSTR(" servo_rx: %"PRIu32"\r\n"), peer_stats.servo_rx);
+ printf_P(PSTR(" servo_tx: %"PRIu32"\r\n"), peer_stats.servo_tx);
+ printf_P(PSTR(" stats_rx: %"PRIu32"\r\n"), peer_stats.stats_rx);
+ printf_P(PSTR(" stats_tx: %"PRIu32"\r\n"), peer_stats.stats_tx);
+ if (peer_stats.echo_ans_rx != 0) {
+ printf_P(PSTR(" echo_ans_latency_ms: %"PRIu32"\r\n"),
+ peer_stats.echo_ans_latency_sum / peer_stats.echo_ans_rx);
+ }
+}
+
+void rc_proto_reset_stats(void)
+{
+ uint8_t prio;
+
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ memset(&stats, 0, sizeof(stats));
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+}
+
+void rc_proto_dump_servos(void)
+{
+ uint8_t i;
+
+ printf_P(PSTR("servo rx\r\n"));
+ for (i = 0; i < N_SERVO; i++) {
+ printf_P(PSTR(" servo[%d] = %d\r\n"), i, servo_rx.servos[i]);
+ }
+ printf_P(PSTR("servo tx\r\n"));
+ printf_P(PSTR(" bypass=%d\r\n"), servo_tx.bypass);
+ printf_P(PSTR(" seq=%d\r\n"), servo_tx.seq);
+ printf_P(PSTR(" time=%d\r\n"), servo_tx.time);
+ for (i = 0; i < N_SERVO; i++) {
+ printf_P(PSTR(" servo[%d] = %d\r\n"), i, servo_tx.servos[i]);
+ }
+}
+
+void rc_proto_set_dstaddr(uint64_t addr)
+{
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ rc_proto_dstaddr = addr;
+ IRQ_UNLOCK(flags);
+}
+
+uint64_t rc_proto_get_dstaddr(void)
+{
+ uint64_t addr;
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ addr = rc_proto_dstaddr;
+ IRQ_UNLOCK(flags);
+ return addr;
+}
+
+void rc_proto_init(void)
+{
+ callout_init(&rc_proto_timer, rc_proto_cb, NULL, XBEE_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &rc_proto_timer, 0);
+}
--- /dev/null
+#ifndef RC_PROTO_H
+#define RC_PROTO_H
+
+/* generic header */
+struct rc_proto_hdr {
+ uint8_t type;
+} __attribute__((packed));
+
+
+/* send a hello message, no answer is expected from the peer */
+#define RC_PROTO_HELLO 0x00
+struct rc_proto_hello {
+ uint8_t type;
+ uint8_t datalen; /* len of data excluding header */
+ uint8_t data[];
+} __attribute__((packed));
+
+/* send an echo request, expect an echo reply from the peer */
+#define RC_PROTO_ECHO_REQ 0x01
+struct rc_proto_echo_req {
+ uint8_t type;
+ int8_t power;
+ uint16_t timestamp; /* copied as-is in answer */
+ uint8_t datalen; /* len of data excluding header */
+ uint8_t data[];
+} __attribute__((packed));
+
+/* reply to an echo request */
+#define RC_PROTO_ECHO_ANS 0x02
+struct rc_proto_echo_ans {
+ uint8_t type;
+ uint16_t timestamp;
+ uint8_t datalen; /* len of data excluding header */
+ uint8_t data[];
+} __attribute__((packed));
+
+/* send a power level probe to the peer: no answer is expected, but the peer
+ * will know that a packet with this power-level is received. It can also ask
+ * the RSSI of this packet. */
+#define RC_PROTO_POWER_PROBE 0x03
+struct rc_proto_power_probe {
+ uint8_t type;
+ uint8_t power_level;
+} __attribute__((packed));
+
+/* acknowledge a servo command */
+#define RC_PROTO_ACK 0x04
+struct rc_proto_ack {
+ uint8_t type;
+ uint8_t seq;
+} __attribute__((packed));
+
+/*
+ * If type < 0x3f, it's a servo command. The size of the message is critical
+ * because it's send very often. The "type" field contains the bitfield of
+ * servos present in the body of the message. A sequence number (5bits) and the
+ * power level (3 bits) are also sent. The servos are listed as 10bits fields,
+ * without padding.
+ *
+ * Example: we send servo 0 (=0x123) and servo 3 (=0x234). Power = 2.
+ * command type (RC_PROTO_SERVO)
+ * power_level = 0 (3 bits, LSB) \
+ * bypass = 0 (1 bit) > one byte
+ * seq = 1 (4 bits, MSB) /
+ * servo_val[0]=0x123
+ * servo_val[1]=0x234
+ * servo_val[2]=0x321
+ * servo_val[3]=0x123
+ * servo_val[4]=0x234
+ * servo_val[5]=0x321
+ *
+ * -> 0x05 0x10 0x48 0xe3 0x4c 0x85 0x23 0x8d 0x32 0x10
+ * type psb |ser0 ser1 ser2 ser3 |ser4 ser5
+ */
+#define RC_PROTO_SERVO 0x05
+#define RC_PROTO_SERVO_LEN 10 /* len of frame */
+struct rc_proto_servo {
+ uint8_t type;
+ uint8_t data[];
+};
+
+/* send stats */
+#define RC_PROTO_STATS 0x06
+struct rc_proto_stats {
+ uint8_t type;
+ uint8_t stats[]; /* format is struct rc_proto_stats_data */
+} __attribute__((packed));
+
+/* rc_proto timers configuration */
+struct rc_proto_timers {
+ uint16_t send_servo_min_ms;
+ uint16_t send_servo_max_ms;
+ uint16_t send_power_probe_ms;
+ uint16_t autobypass_ms;
+};
+extern struct rc_proto_timers rc_proto_timers;
+
+/* send a Hello message to a peer */
+int8_t rc_proto_send_hello(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power);
+
+int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len,
+ int8_t power);
+
+/* reception of a xbee message */
+int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len);
+
+/* dump statistics related to rc_proto */
+void rc_proto_dump_stats(void);
+
+/* reset statistics related to rc_proto */
+void rc_proto_reset_stats(void);
+
+/* set the peer xbee address */
+void rc_proto_set_dstaddr(uint64_t addr);
+
+/* get the peer xbee address */
+uint64_t rc_proto_get_dstaddr(void);
+
+void rc_proto_dump_servos(void);
+
+/* tx mode */
+#define RC_PROTO_FLAGS_TX_OFF 0x00
+#define RC_PROTO_FLAGS_TX_BYPASS 0x01
+#define RC_PROTO_FLAGS_TX_COPY_SPI 0x02
+#define RC_PROTO_FLAGS_TX_RESERVED 0x03
+#define RC_PROTO_FLAGS_TX_MASK 0x03
+
+/* if set, copy received servo values to SPI */
+#define RC_PROTO_FLAGS_RX_COPY_SPI 0x04
+
+/* if set, switch to bypass when no servo is received during some time */
+#define RC_PROTO_FLAGS_RX_AUTOBYPASS 0x08
+
+/* if set, send stats periodically to the peer (1 sec) */
+#define RC_PROTO_FLAGS_TX_STATS 0x10
+
+/* if set, send power probe periodically to the peer (500 ms) */
+#define RC_PROTO_FLAGS_TX_POW_PROBE 0x20
+
+/* if set, use received probes to compute best power level */
+#define RC_PROTO_FLAGS_COMPUTE_BEST_POW 0x40
+
+void rc_proto_set_mode(uint8_t flags);
+
+uint8_t rc_proto_get_mode(void);
+
+/* initialize rc_proto module */
+void rc_proto_init(void);
+
+#endif
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: scheduler_config.h,v 1.2 2009-03-15 20:18:33 zer0 Exp $
+ *
+ */
+
+#ifndef _SCHEDULER_CONFIG_H_
+#define _SCHEDULER_CONFIG_H_
+
+#define _SCHEDULER_CONFIG_VERSION_ 4
+
+/** maximum number of allocated events */
+#define SCHEDULER_NB_MAX_EVENT 10
+
+#ifdef HOST_VERSION
+/* #define SCHEDULER_UNIT_FLOAT 1000.0 */
+/* #define SCHEDULER_UNIT 1000UL */
+#else
+/* tim0_div * tim0_res * soft_presc / quartz
+ * = (8 * 256 * 4) / 12 */
+#define SCHEDULER_UNIT_FLOAT 682.0
+#define SCHEDULER_UNIT 682L
+#endif
+
+/** number of allowed imbricated scheduler interrupts. The maximum
+ * should be SCHEDULER_NB_MAX_EVENT since we never need to imbricate
+ * more than once per event. If it is less, it can avoid to browse the
+ * event table, events are delayed (we loose precision) but it takes
+ * less CPU */
+#define SCHEDULER_NB_STACKING_MAX SCHEDULER_NB_MAX_EVENT
+
+/** define it for debug infos (not recommended, because very slow on
+ * an AVR, it uses printf in an interrupt). It can be useful if
+ * prescaler is very high, making the timer interrupt period very
+ * long in comparison to printf() */
+/* #define SCHEDULER_DEBUG */
+
+#endif // _SCHEDULER_CONFIG_H_
--- /dev/null
+/*
+ * Copyright Droids Corporation (2008)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/*
+ * Author : Julien LE GUEN - jlg@jleguen.info
+ */
+
+
+/*
+ * Configure HERE your SPI module
+ */
+
+
+
+/* Number of slave devices in your system
+ * Each slave have a dedicated SS line that you have to register
+ * before using the SPI module
+ */
+#define SPI_MAX_SLAVES 1
+
--- /dev/null
+#include <stdint.h>
+#include <stdio.h>
+
+#include <aversive.h>
+#include <aversive/wait.h>
+
+#include <callout.h>
+
+#include "spi_servo.h"
+#include "main.h"
+
+/*
+ * The goal of this code is to send the servo commands to the slave
+ * through the SPI bus. As the slave runs in polling mode to be
+ * precise when generating servo signals, we cannot send data very
+ * fast. We send one byte every ms, this is enough as we have at most
+ * 6 servos (2 bytes) to update every 20ms
+ *
+ * When a new servo value is received, we send the first byte to the
+ * SPI bus and store the next one. It will be transmitted by a
+ * callback 1ms later. If new servos values are received during this
+ * time, they are just saved but not transmitted until the first
+ * command is issued. Once all commands have been transmitted, the
+ * callback is unloaded.
+ */
+
+#define PPM_BIT 0x01
+#define BYPASS_BIT 0x02
+
+struct spi_servo_tx {
+ uint16_t servo[N_SERVO+1]; /* one more for control channel */
+ uint16_t cmd_mask;
+ uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
+ uint8_t cur_idx;
+};
+static struct spi_servo_tx spi_servo_tx;
+
+struct spi_servo_rx {
+ uint16_t servo[N_SERVO];
+ uint8_t prev_byte;
+};
+static struct spi_servo_rx spi_servo_rx;
+
+static struct callout spi_timer;
+
+/*
+ * SPI protocol:
+ *
+ * A command is stored on 2 bytes. The first one has its msb to 0, and the
+ * second one to 1. The first received byte contains the command number, and the
+ * msb of the servo value. The second byte contains the lsb of the servo value.
+ *
+ * Command 0 is only one byte long, it means "I have nothing to say".
+ * Commands 1 to N_SERVO (included) are to set the value of servo.
+ * Command N_SERVO+1 is:
+ * - to enable/disable ppm generation in place of last servo.
+ * - to enable/disable bypass mode
+ */
+union spi_byte0 {
+ uint8_t u8;
+ struct {
+ /* inverted: little endian */
+ uint8_t val_msb:3;
+ uint8_t cmd_num:4;
+ uint8_t zero:1;
+ };
+};
+
+union spi_byte1 {
+ uint8_t u8;
+ struct {
+ /* inverted: little endian */
+ uint8_t val_lsb:7;
+ uint8_t one:1;
+ };
+};
+
+#define SS_HIGH() PORTB |= (1 << 4)
+#define SS_LOW() PORTB &= (~(1 << 4))
+
+static void spi_send_byte(uint8_t byte)
+{
+ SS_LOW();
+ SPDR = byte;
+ /* Wait for transmission complete (active loop is fine because
+ * the clock is high) */
+ while(!(SPSR & (1<<SPIF)));
+ SS_HIGH();
+}
+
+static void spi_send_one_servo(uint8_t num, uint16_t val)
+{
+ union spi_byte0 byte0;
+ union spi_byte1 byte1;
+
+ byte0.val_msb = val >> 7;
+ byte0.cmd_num = num + 1;
+ byte0.zero = 0;
+ byte1.one = 1;
+ byte1.val_lsb = val;
+
+ /* save the second byte */
+ spi_servo_tx.next_byte = byte1.u8;
+
+ /* send the first byte */
+ spi_send_byte(byte0.u8);
+}
+
+static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
+{
+ uint8_t num;
+ uint16_t val;
+
+ num = byte0.cmd_num - 1;
+ if (num >= N_SERVO)
+ return;
+
+ val = byte0.val_msb;
+ val <<= 7;
+ val |= byte1.val_lsb;
+
+ spi_servo_rx.servo[num] = val;
+}
+
+/* called by the scheduler */
+static void spi_servo_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
+{
+ uint8_t idx;
+ union spi_byte0 byte0;
+ union spi_byte1 byte1;
+
+ (void)arg;
+
+ /* get the value from the slave */
+ byte0.u8 = SPDR;
+ byte1.u8 = byte0.u8;
+ if (byte0.zero == 0) {
+ spi_servo_rx.prev_byte = byte0.u8;
+ }
+ else {
+ byte0.u8 = spi_servo_rx.prev_byte;
+ decode_rx_servo(byte0, byte1);
+ }
+
+ /* if next byte is set, send it */
+ if (spi_servo_tx.next_byte != 0) {
+ spi_send_byte(spi_servo_tx.next_byte);
+ spi_servo_tx.next_byte = 0;
+ goto reschedule;
+ }
+
+ /* if there is no updated servo, send 0 and return. */
+ if (spi_servo_tx.cmd_mask == 0) {
+ spi_send_byte(0);
+ goto reschedule;
+ }
+
+ /* else find it and send it */
+ idx = spi_servo_tx.cur_idx;
+ while (1) {
+ idx++;
+ if (idx > N_SERVO + 1)
+ idx = 0;
+
+ if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
+ break;
+ }
+
+ spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
+ spi_servo_tx.cmd_mask &= (~(1 << idx));
+ spi_servo_tx.cur_idx = idx;
+
+ reschedule:
+ /* don't use callout_reschedule() here, we want to schedule in one tick
+ * relative to current time: 1 tick is 682us at 12Mhz */
+ callout_schedule(cm, tim, 0);
+}
+
+void spi_servo_init(void)
+{
+ /* SCK, SS & MOSI */
+ DDRB = 0xb0;
+
+ /* remove power reduction on spi */
+ PRR0 &= ~(1 << PRSPI);
+
+ /* Enable SPI, Master, set clock rate fck/16 */
+ SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR0);
+
+ SS_HIGH();
+
+ callout_init(&spi_timer, spi_servo_cb, NULL, SPI_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &spi_timer, 0); /* immediate */
+ spi_servo_set_bypass(1);
+}
+
+void spi_servo_set(uint8_t num, uint16_t val)
+{
+ uint8_t flags;
+
+ if (num >= N_SERVO)
+ return;
+
+ IRQ_LOCK(flags);
+ spi_servo_tx.servo[num] = val;
+ spi_servo_tx.cmd_mask |= (1 << num);
+ IRQ_UNLOCK(flags);
+}
+
+uint16_t spi_servo_get(uint8_t num)
+{
+ uint8_t flags;
+ uint16_t val;
+
+ if (num >= N_SERVO)
+ return 0;
+
+ IRQ_LOCK(flags);
+ val = spi_servo_rx.servo[num];
+ IRQ_UNLOCK(flags);
+
+ return val;
+}
+
+uint8_t spi_servo_get_bypass(void)
+{
+ return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT);
+}
+
+uint8_t spi_servo_get_ppm(void)
+{
+ return !!(spi_servo_tx.servo[N_SERVO] & PPM_BIT);
+}
+
+void spi_servo_set_bypass(uint8_t enable)
+{
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+ if (enable)
+ spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT;
+ else
+ spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT);
+ spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+ IRQ_UNLOCK(flags);
+}
+
+void spi_servo_set_ppm(uint8_t enable)
+{
+ uint8_t flags;
+
+ IRQ_LOCK(flags);
+ spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+ if (enable)
+ spi_servo_tx.servo[N_SERVO] |= PPM_BIT;
+ else
+ spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT);
+ spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+ IRQ_UNLOCK(flags);
+}
+
+void spi_servo_dump(void)
+{
+ uint8_t i;
+
+ for (i = 0; i < N_SERVO; i++)
+ printf_P(PSTR("%d: rx=%4.4d tx=%4.4d\r\n"), i,
+ spi_servo_get(i), spi_servo_tx.servo[i]);
+ printf_P(PSTR("bypass=%d ppm=%d\n"),
+ spi_servo_get_bypass(), spi_servo_get_ppm());
+}
--- /dev/null
+#define N_SERVO 6
+
+void spi_servo_init(void);
+void spi_servo_set(uint8_t num, uint16_t val);
+uint16_t spi_servo_get(uint8_t num);
+void spi_servo_set_bypass(uint8_t enable);
+void spi_servo_set_ppm(uint8_t enable);
+uint8_t spi_servo_get_bypass(void);
+uint8_t spi_servo_get_ppm(void);
+void spi_servo_dump(void);
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: time_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $
+ *
+ */
+
+/** precision of the time processor, in us */
+#define TIME_PRECISION 25000l
--- /dev/null
+/*
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2006)
+ *
+ * This program 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 of the License, or
+ * (at your option) any later version.
+ *
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ * Revision : $Id: timer_config.h,v 1.1 2009-02-20 21:10:01 zer0 Exp $
+ *
+ */
+
+#define TIMER0_ENABLED
+
+/* #define TIMER1_ENABLED */
+/* #define TIMER1A_ENABLED */
+/* #define TIMER1B_ENABLED */
+/* #define TIMER1C_ENABLED */
+
+/* #define TIMER2_ENABLED */
+
+/* #define TIMER3_ENABLED */
+/* #define TIMER3A_ENABLED */
+/* #define TIMER3B_ENABLED */
+/* #define TIMER3C_ENABLED */
+
+#define TIMER0_PRESCALER_DIV 8
--- /dev/null
+/*\r
+ * Copyright Droids Corporation, Microb Technology, Eirbot (2005)\r
+ *\r
+ * This program is free software; you can redistribute it and/or modify\r
+ * it under the terms of the GNU General Public License as published by\r
+ * the Free Software Foundation; either version 2 of the License, or\r
+ * (at your option) any later version.\r
+ *\r
+ * This program is distributed in the hope that it will be useful,\r
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of\r
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\r
+ * GNU General Public License for more details.\r
+ *\r
+ * You should have received a copy of the GNU General Public License\r
+ * along with this program; if not, write to the Free Software\r
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA\r
+ *\r
+ * Revision : $Id: uart_config.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $\r
+ *\r
+ */\r
+\r
+/* Droids-corp 2004 - Zer0\r
+ * config for uart module\r
+ */\r
+\r
+#ifndef UART_CONFIG_H\r
+#define UART_CONFIG_H\r
+\r
+/*\r
+ * UART0 definitions\r
+ */\r
+\r
+/* compile uart0 fonctions, undefine it to pass compilation */\r
+#define UART0_COMPILE\r
+\r
+/* enable uart0 if == 1, disable if == 0 */\r
+#define UART0_ENABLED 1\r
+\r
+/* enable uart0 interrupts if == 1, disable if == 0 */\r
+#define UART0_INTERRUPT_ENABLED 1\r
+\r
+#define UART0_BAUDRATE 57600\r
+\r
+/*\r
+ * if you enable this, the maximum baudrate you can reach is\r
+ * higher, but the precision is lower.\r
+ */\r
+#define UART0_USE_DOUBLE_SPEED 1\r
+\r
+#define UART0_RX_FIFO_SIZE 64\r
+#define UART0_TX_FIFO_SIZE 127\r
+#define UART0_NBITS 8\r
+\r
+#define UART0_PARITY UART_PARTITY_NONE\r
+\r
+#define UART0_STOP_BIT UART_STOP_BITS_1\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+/*\r
+ * UART1 definitions\r
+ */\r
+\r
+/* compile uart1 fonctions, undefine it to pass compilation */\r
+#define UART1_COMPILE\r
+\r
+/* enable uart1 if == 1, disable if == 0 */\r
+#define UART1_ENABLED 1\r
+\r
+/* enable uart1 interrupts if == 1, disable if == 0 */\r
+#define UART1_INTERRUPT_ENABLED 1\r
+\r
+#define UART1_BAUDRATE 57600\r
+\r
+/*\r
+ * if you enable this, the maximum baudrate you can reach is\r
+ * higher, but the precision is lower.\r
+ */\r
+#define UART1_USE_DOUBLE_SPEED 1\r
+\r
+#define UART1_RX_FIFO_SIZE 64\r
+#define UART1_TX_FIFO_SIZE 127\r
+#define UART1_NBITS 8\r
+\r
+#define UART1_PARITY UART_PARTITY_NONE\r
+\r
+#define UART1_STOP_BIT UART_STOP_BITS_1\r
+\r
+/* .... same for uart 1, 2, 3 ... */\r
+\r
+#endif\r
+\r
--- /dev/null
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stddef.h>
+#include <ctype.h>
+
+#include <aversive.h>
+#include <aversive/endian.h>
+#include <aversive/wait.h>
+#include <aversive/pgmspace.h>
+
+#include <callout.h>
+#include <rdline.h>
+#include <xbee.h>
+#include <xbee_rxtx.h>
+
+#include "rc_proto.h"
+#include "xbee_user.h"
+#include "main.h"
+
+#define XBEE_TIMEOUT_MS 1000
+#define XBEE_POLL_TIMER_MS 5
+
+static struct xbee_ctx xbee_ctx[XBEE_MAX_CHANNEL];
+
+int xbee_cmdline_input_enabled = 1;
+
+/* parameters */
+int xbee_raw = 0;
+
+static struct callout xbee_rx_poll_timer;
+
+static void __hexdump(const void *buf, unsigned int len)
+{
+ unsigned int i, out, ofs;
+ const unsigned char *data = buf;
+#define LINE_LEN 80
+ char line[LINE_LEN]; /* space needed 8+16*3+3+16 == 75 */
+
+ ofs = 0;
+ while (ofs < len) {
+ /* format 1 line in the buffer, then use printk to print them */
+ out = snprintf_P(line, LINE_LEN, PSTR("%08X"), ofs);
+ for (i=0; ofs+i < len && i<16; i++)
+ out += snprintf_P(line+out, LINE_LEN - out,
+ PSTR(" %02X"),
+ data[ofs+i]&0xff);
+ for (;i<=16;i++)
+ out += snprintf(line+out, LINE_LEN - out, " ");
+ for (i=0; ofs < len && i<16; i++, ofs++) {
+ unsigned char c = data[ofs];
+ if (!isascii(c) || !isprint(c))
+ c = '.';
+ out += snprintf_P(line+out,
+ LINE_LEN - out,
+ PSTR("%c"), c);
+ }
+ DEBUG(E_USER_XBEE, "%s", line);
+ }
+}
+
+static void hexdump_msg(const char *title, const struct xbee_msg *msg)
+{
+ unsigned i;
+
+ DEBUG(E_USER_XBEE, "dump %s", title);
+
+ for (i = 0; i < msg->iovlen; i++) {
+ DEBUG(E_USER_XBEE, "iovec %d at %p, len=%d", i,
+ msg->iov[i].buf, msg->iov[i].len);
+ __hexdump(msg->iov[i].buf, msg->iov[i].len);
+ }
+}
+
+void hexdump(const char *title, const void *buf, unsigned int len)
+{
+ DEBUG(E_USER_XBEE, "dump %s at [%p], len=%d", title, buf, len);
+ __hexdump(buf, len);
+}
+
+static int parse_xmit_status(struct xbee_ctx *ctx,
+ struct xbee_xmit_status_hdr *frame, unsigned len)
+{
+ (void)len;
+
+ if (ctx == NULL) {
+ ERROR(E_USER_XBEE, "no context");
+ return -1;
+ }
+
+ /* see if it matches a xmit query (atcmd_query must be \0) */
+ if (ctx->atcmd_query[0] != '\0') {
+ ERROR(E_USER_XBEE, "invalid response 2");
+ return -1;
+ }
+
+ /* XXX use defines for these values */
+ if (frame->delivery_status == 0x00)
+ NOTICE(E_USER_XBEE, "Success");
+ else if (frame->delivery_status == 0x01)
+ WARNING(E_USER_XBEE, "MAC ACK Failure");
+ else if (frame->delivery_status == 0x15)
+ WARNING(E_USER_XBEE, "Invalid destination endpoint");
+ else if (frame->delivery_status == 0x21)
+ WARNING(E_USER_XBEE, "Network ACK Failure");
+ else if (frame->delivery_status == 0x25)
+ WARNING(E_USER_XBEE, "Route Not Found");
+
+ return 0;
+}
+
+/* Write in a human readable format the content of an atcmd response frame. It
+ * assumes the frame is valid .*/
+void atresp_to_str(char *buf, unsigned buflen, const struct xbee_atresp_hdr *frame,
+ unsigned len)
+{
+ union {
+ uint8_t u8;
+ uint16_t u16;
+ uint32_t u32;
+ int16_t s16;
+ } __attribute__((packed)) *result;
+ char atcmd_str[3];
+ const struct xbee_atcmd *cmd_pgm;
+ struct xbee_atcmd cmd;
+
+ /* get AT command from frame */
+ memcpy(atcmd_str, &frame->cmd, 2);
+ atcmd_str[2] = '\0';
+
+
+ /* see if it exists */
+ cmd_pgm = xbee_atcmd_lookup_name(atcmd_str);
+ if (cmd_pgm == NULL) {
+ snprintf(buf, buflen, "<%s> (unknown cmd)", atcmd_str);
+ return;
+ }
+ memcpy_P(&cmd, cmd_pgm, sizeof(cmd));
+ len -= sizeof(*frame);
+
+ /* dump frame */
+ result = (void *)frame->data;
+
+ if (cmd.flags & XBEE_ATCMD_F_PARAM_U8 && len == sizeof(uint8_t))
+ snprintf(buf, buflen, "<%s> is 0x%x (%d)", atcmd_str,
+ result->u8, result->u8);
+ else if (cmd.flags & XBEE_ATCMD_F_PARAM_U16 && len == sizeof(uint16_t))
+ snprintf(buf, buflen, "<%s> is 0x%x (%d)", atcmd_str,
+ ntohs(result->u16), ntohs(result->u16));
+ else if (cmd.flags & XBEE_ATCMD_F_PARAM_U32 && len == sizeof(uint32_t))
+ snprintf(buf, buflen, "<%s> is 0x%"PRIx32" (%"PRIu32")",
+ atcmd_str, ntohl(result->u32), ntohl(result->u32));
+ else if (cmd.flags & XBEE_ATCMD_F_PARAM_S16 && len == sizeof(int16_t))
+ snprintf(buf, buflen, "<%s> is %d",
+ atcmd_str, ntohs(result->s16));
+ else if (len == 0)
+ snprintf(buf, buflen, "<%s> no data", atcmd_str);
+ else
+ snprintf(buf, buflen, "invalid atresp");
+}
+
+static int parse_atcmd(struct xbee_ctx *ctx, struct xbee_atresp_hdr *frame,
+ unsigned len)
+{
+ char atcmd_str[3];
+ char buf[32];
+
+ if (ctx == NULL) {
+ ERROR(E_USER_XBEE, "no context");
+ return -1;
+ }
+
+ /* get AT command from frame */
+ memcpy(atcmd_str, &frame->cmd, 2);
+ atcmd_str[2] = '\0';
+
+ /* see if it matches query */
+ if (memcmp(atcmd_str, ctx->atcmd_query, 2)) {
+ ERROR(E_USER_XBEE, "invalid response <%c%c><%s><%s>",
+ frame->cmd & 0xFF,
+ (frame->cmd >> 8) & 0xFF,
+ atcmd_str, ctx->atcmd_query);
+ return -1;
+ }
+
+ /* bad status */
+ if (frame->status == 1) {
+ WARNING(E_USER_XBEE, "Status is error");
+ return -1;
+ }
+ else if (frame->status == 2) {
+ WARNING(E_USER_XBEE, "Invalid command");
+ return -1;
+ }
+ else if (frame->status == 3) {
+ WARNING(E_USER_XBEE, "Invalid parameter");
+ return -1;
+ }
+ else if (frame->status != 0) {
+ WARNING(E_USER_XBEE, "Unknown status error %d",
+ frame->status);
+ return -1;
+ }
+
+ atresp_to_str(buf, sizeof(buf), frame, len);
+
+ len -= sizeof(*frame);
+ NOTICE(E_USER_XBEE, "status ok, datalen=%d, %s", len, buf);
+
+ if (len != 0)
+ hexdump("atcmd answer", frame->data, len);
+
+ return 0;
+}
+
+
+/* Main xbee rx entry point for application. It decodes the xbee frame type and
+ * dispatch to the application layer. Then "len" argument does not include the
+ * xbee_hdr structure (delimiter, len, type, id) and checksum. */
+int8_t xbeeapp_rx(struct xbee_dev *dev, int channel, int type,
+ void *frame, unsigned len, void *opaque)
+{
+ struct xbee_ctx *ctx = opaque;
+ int8_t ret = XBEE_USER_RETCODE_OK;
+
+ NOTICE(E_USER_XBEE, "type=0x%x, channel=%d, ctx=%p",
+ type, channel, ctx);
+ __hexdump(frame, len);
+
+ /* if ctx is !NULL, it is an answer to a query */
+ if (ctx != NULL) {
+ xbee_unload_timeout(ctx);
+ if (ctx->atcmd_query[0])
+ NOTICE(E_USER_XBEE, "Received answer to query <%c%c>",
+ ctx->atcmd_query[0], ctx->atcmd_query[1]);
+ }
+
+ /* some additional checks before sending */
+ switch (type) {
+ case XBEE_TYPE_MODEM_STATUS: {
+ NOTICE(E_USER_XBEE, "Received Modem Status frame");
+ break;
+ }
+
+ case XBEE_TYPE_RMT_ATRESP: {
+ union {
+ uint64_t u64;
+ struct {
+#if BYTE_ORDER == LITTLE_ENDIAN
+ uint32_t low;
+ uint32_t high;
+#else
+ uint32_t high;
+ uint32_t low;
+#endif
+ } u32;
+ } addr;
+ memcpy(&addr, frame, sizeof(addr));
+ addr.u64 = ntohll(addr.u64);
+ NOTICE(E_USER_XBEE,
+ "from remote address %"PRIx32"%"PRIx32"",
+ addr.u32.high, addr.u32.low);
+
+ /* this answer contains an atcmd answer at offset 10 */
+ if (parse_atcmd(ctx, frame + 10, len - 10) < 0)
+ ret = XBEE_USER_RETCODE_BAD_FRAME;
+
+ break;
+ }
+ case XBEE_TYPE_ATRESP: {
+ if (parse_atcmd(ctx, frame, len) < 0)
+ ret = XBEE_USER_RETCODE_BAD_FRAME;
+
+ break;
+ }
+
+ case XBEE_TYPE_XMIT_STATUS: {
+ if (parse_xmit_status(ctx, frame, len) < 0)
+ ret = XBEE_USER_RETCODE_BAD_FRAME;
+
+ break;
+ }
+
+ case XBEE_TYPE_RECV: {
+ if (rc_proto_rx(frame, len) < 0)
+ ret = XBEE_USER_RETCODE_BAD_FRAME;
+
+ break;
+ }
+
+ case XBEE_TYPE_ATCMD:
+ case XBEE_TYPE_ATCMD_Q:
+ case XBEE_TYPE_XMIT:
+ case XBEE_TYPE_EXPL_XMIT:
+ case XBEE_TYPE_RMT_ATCMD:
+ case XBEE_TYPE_EXPL_RECV:
+ case XBEE_TYPE_NODE_ID:
+ default:
+ ERROR(E_USER_XBEE, "Invalid frame");
+ ret = XBEE_USER_RETCODE_BAD_FRAME;
+ break;
+ }
+
+ if (ret != XBEE_USER_RETCODE_OK) {
+ WARNING(E_USER_XBEE, "undecoded rx frame");
+ }
+
+ if (ctx != NULL) {
+ /* callback */
+ if (ctx->rx_cb != NULL)
+ ret = ctx->rx_cb(ret, frame, len, ctx->arg);
+
+ /* free channel now, it implicitely frees the context too */
+ xbee_unregister_channel(dev, channel);
+ }
+
+ return ret;
+}
+
+/* called with callout prio == XBEE_PRIO */
+static int xbeeapp_send(struct xbee_ctx *ctx, int type, struct xbee_msg *msg)
+{
+ int ret;
+ int channel;
+
+ /* register a channel */
+ channel = xbee_register_channel(xbee_dev, XBEE_CHANNEL_ANY,
+ xbeeapp_rx, NULL);
+ if (channel < 0) {
+ ERROR(E_USER_XBEE, "cannot send: no free channel");
+ return -1;
+ }
+
+ /* copy context in the static struct table (avoiding a malloc) */
+ memcpy(&xbee_ctx[channel], ctx, sizeof(*ctx));
+ ctx = &xbee_ctx[channel];
+ xbee_set_opaque(xbee_dev, channel, ctx);
+
+ NOTICE(E_USER_XBEE, "send frame channel=%d type=0x%x", channel, type);
+ hexdump_msg("xmit frame", msg);
+
+ /* transmit the frame on this channel */
+ ret = xbee_tx_iovec(xbee_dev, channel, type, msg);
+ if (ret < 0) {
+ ERROR(E_USER_XBEE, "cannot send");
+ xbee_unregister_channel(xbee_dev, channel);
+ return -1;
+ }
+
+ ctx->channel = channel;
+ xbee_load_timeout(ctx); /* load a timeout event */
+
+ return 0;
+}
+
+/* send an AT command with parameters filled by caller. param is the argument
+ * of the atcmd (if any). */
+int xbeeapp_send_atcmd(char *atcmd_str, void *param, unsigned param_len,
+ xbee_user_rx_cb_t *rx_cb, void *arg)
+{
+ struct xbee_ctx ctx;
+ /* struct xbee_atcmd_hdr atcmd_hdr; -> no needed same than atcmd_str */
+ struct xbee_msg msg;
+ uint8_t prio;
+ int ret;
+
+ memset(&ctx, 0, sizeof(ctx));
+ ctx.atcmd_query[0] = atcmd_str[0];
+ ctx.atcmd_query[1] = atcmd_str[1];
+ ctx.rx_cb = rx_cb;
+ ctx.arg = arg;
+
+ if (param_len == 0) {
+ msg.iovlen = 1;
+ msg.iov[0].buf = atcmd_str;
+ msg.iov[0].len = 2;
+ }
+ else {
+ msg.iovlen = 2;
+ msg.iov[0].buf = atcmd_str;
+ msg.iov[0].len = 2;
+ msg.iov[1].buf = param;
+ msg.iov[1].len = param_len;
+ }
+
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ if (prio > XBEE_PRIO)
+ ERROR(E_USER_XBEE, "invalid prio = %d\n", prio);
+ ret = xbeeapp_send(&ctx, XBEE_TYPE_ATCMD, &msg);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+int xbeeapp_send_msg(uint64_t addr, struct xbee_msg *msg,
+ xbee_user_rx_cb_t *rx_cb, void *arg)
+{
+ struct xbee_ctx ctx;
+ struct xbee_xmit_hdr xmit_hdr;
+ struct xbee_msg msg2;
+ unsigned i;
+ uint8_t prio;
+ int ret;
+
+ if (msg->iovlen + 2 > XBEE_MSG_MAXIOV) {
+ ERROR(E_USER_XBEE, "too many iovecs");
+ return -1;
+ }
+
+ xmit_hdr.dstaddr = htonll(addr);
+ xmit_hdr.reserved = htons(0xFFFE);
+ xmit_hdr.bcast_radius = 0;
+ xmit_hdr.opts = 0;
+
+ msg2.iovlen = msg->iovlen + 1;
+ msg2.iov[0].buf = &xmit_hdr;
+ msg2.iov[0].len = sizeof(xmit_hdr);
+ for (i = 0; i < msg->iovlen; i++)
+ msg2.iov[i+1] = msg->iov[i];
+
+ memset(&ctx, 0, sizeof(ctx));
+ ctx.rx_cb = rx_cb;
+ ctx.arg = arg;
+
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ if (prio > XBEE_PRIO)
+ ERROR(E_USER_XBEE, "invalid prio = %d\n", prio);
+ ret = xbeeapp_send(&ctx, XBEE_TYPE_XMIT, &msg2);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+
+ return ret;
+}
+
+static void evt_timeout(struct callout_mgr *cm, struct callout *clt,
+ void *arg)
+{
+ struct xbee_ctx *ctx = arg;
+
+ (void)cm;
+ (void)clt;
+
+ WARNING(E_USER_XBEE, "Timeout");
+
+ /* callback */
+ if (ctx->rx_cb != NULL)
+ ctx->rx_cb(XBEE_USER_RETCODE_TIMEOUT, NULL, 0, ctx->arg);
+
+ /* free channel, it implicitely frees the context too */
+ xbee_unregister_channel(xbee_dev, ctx->channel);
+ callout_stop(cm, clt);
+}
+
+void xbee_load_timeout(struct xbee_ctx *ctx)
+{
+ uint8_t prio;
+
+ callout_init(&ctx->timeout, evt_timeout, ctx, XBEE_PRIO);
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &ctx->timeout, XBEE_TIMEOUT_MS);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+}
+
+void xbee_unload_timeout(struct xbee_ctx *ctx)
+{
+ uint8_t prio;
+
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+ callout_stop(&xbeeboard.intr_cm, &ctx->timeout);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+}
+
+static void xbee_rx_poll_timer_cb(struct callout_mgr *cm, struct callout *tim,
+ void *arg)
+{
+ (void) arg;
+ xbee_rx(xbee_dev);
+ callout_reschedule(cm, tim, XBEE_POLL_TIMER_MS);
+}
+
+void xbee_mainloop(void)
+{
+ uint8_t prio;
+
+ while (1) {
+ if (xbee_raw) {
+ int16_t c;
+
+ /* from xbee to cmdline */
+ c = xbee_dev_recv(NULL);
+ if (c >= 0)
+ cmdline_dev_send((uint8_t)c, NULL);
+
+ /* from cmdline to xbee */
+ c = cmdline_dev_recv(NULL);
+ if (c == 4) { /* CTRL-d */
+ prio = callout_mgr_set_prio(&xbeeboard.intr_cm,
+ XBEE_PRIO);
+ xbee_dev_send('A', NULL);
+ xbee_dev_send('T', NULL);
+ xbee_dev_send('C', NULL);
+ xbee_dev_send('N', NULL);
+ xbee_dev_send('\n', NULL);
+ callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
+ xbee_raw = 0;
+ rdline_newline(&xbeeboard.rdl,
+ xbeeboard.prompt);
+ }
+ else if (c >= 0) {
+ /* send to xbee */
+ xbee_dev_send((uint8_t)c, NULL);
+
+ /* echo on cmdline */
+ cmdline_dev_send((uint8_t)c, NULL);
+ }
+ }
+ else {
+ if (xbee_cmdline_input_enabled)
+ cmdline_poll();
+ /* xbee rx polling is done in a timer, so we can block
+ * the cmdline without loosing incoming packets */
+ }
+ }
+}
+
+void xbee_stdin_enable(void)
+{
+ xbee_cmdline_input_enabled = 1;
+}
+
+void xbee_stdin_disable(void)
+{
+ xbee_cmdline_input_enabled = 0;
+}
+
+void xbeeapp_init(void)
+{
+ callout_init(&xbee_rx_poll_timer, xbee_rx_poll_timer_cb,
+ NULL, XBEE_PRIO);
+ callout_schedule(&xbeeboard.intr_cm, &xbee_rx_poll_timer,
+ XBEE_POLL_TIMER_MS);
+}
--- /dev/null
+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * All rights reserved.
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef _XBEE_USER_H_
+#define _XBEE_USER_H_
+
+#include <callout.h>
+#include <xbee_rxtx.h>
+
+#define XBEE_USER_RETCODE_OK 0
+#define XBEE_USER_RETCODE_BAD_FRAME 1
+#define XBEE_USER_RETCODE_TIMEOUT 2
+
+/* called when we receive an answer to a query */
+typedef int8_t (xbee_user_rx_cb_t)(int8_t code, void *frame,
+ unsigned len, void *arg);
+
+/* used for timeouts and xbee rx callback */
+struct xbee_ctx {
+ int channel;
+ char atcmd_query[2]; /* 00 is it's data */
+ xbee_user_rx_cb_t *rx_cb;
+ void *arg;
+ struct callout timeout;
+};
+
+//extern cmdline_parse_ctx_t main_ctx;
+extern struct xbee_dev *xbee_dev;
+extern int xbee_raw;
+
+/* we use a specific structure to send packets. It allows to prepend some
+ * data in the frame without doing a copy. */
+struct xbeeapp_pkt {
+ char *buf;
+ unsigned len;
+ unsigned headroom;
+ unsigned tailroom;
+};
+
+/* Write in a human readable format the content of an atcmd response frame. It
+ * assumes the frame is valid .*/
+void atresp_to_str(char *buf, unsigned buflen, const struct xbee_atresp_hdr *frame,
+ unsigned len);
+
+/* callback registered to xbee module, called when a xbee frame is received */
+int8_t xbeeapp_rx(struct xbee_dev *dev, int channel, int type,
+ void *frame, unsigned len, void *opaque);
+
+/* Send an AT command to the xbee device. The callback function for the answer
+ * is given as a parameter */
+int xbeeapp_send_atcmd(char *atcmd_str, void *param,
+ unsigned param_len, xbee_user_rx_cb_t *rx_cb, void *arg);
+
+/* send a message to a peer */
+int xbeeapp_send_msg(uint64_t addr, struct xbee_msg *msg,
+ xbee_user_rx_cb_t *rx_cb, void *arg);
+
+void xbee_stdin_enable(void);
+void xbee_stdin_disable(void);
+
+void xbee_load_timeout(struct xbee_ctx *ctx);
+void xbee_unload_timeout(struct xbee_ctx *ctx);
+
+void xbee_mainloop(void);
+
+void xbeeapp_init(void);
+
+#endif /* _XBEE_USER_H_ */
git.droids-corp.org / fpv.git / commitdiff