From: Olivier Matz Date: Thu, 24 Jul 2014 16:26:52 +0000 (+0200) Subject: Initial revision X-Git-Url: http://git.droids-corp.org/?p=fpv.git;a=commitdiff_plain;h=9a2241fcc428e58d0c208d1dc3b6c27a8481cd98 Initial revision Start from protos/imu 950c56ac3c1e5651f54965700f385076ab63c8ea protos/xbee-avr 9a090567b4bf503a5919673e7416f3bc7963f3e4 --- 9a2241fcc428e58d0c208d1dc3b6c27a8481cd98 diff --git a/common/i2c_commands.h b/common/i2c_commands.h new file mode 100644 index 0000000..4f4953c --- /dev/null +++ b/common/i2c_commands.h @@ -0,0 +1,76 @@ +/* + * 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_ */ diff --git a/common/todo.txt b/common/todo.txt new file mode 100644 index 0000000..53ac9d6 --- /dev/null +++ b/common/todo.txt @@ -0,0 +1,3 @@ +- printf -> printf_P +- check log rate limitation +- eeprom check to avoid programming the mcu with the prog of another board diff --git a/imuboard/IMU_Razor9DOF.py b/imuboard/IMU_Razor9DOF.py new file mode 100644 index 0000000..91351ae --- /dev/null +++ b/imuboard/IMU_Razor9DOF.py @@ -0,0 +1,146 @@ +# Test for Razor 9DOF IMU +# Jose Julio @2009 +# This script needs VPhyton, pyserial and pywin modules + +# First Install Python 2.6.4 +# Install pywin from http://sourceforge.net/projects/pywin32/ +# Install pyserial from http://sourceforge.net/projects/pyserial/files/ +# Install Vphyton from http://vpython.org/contents/download_windows.html + +from visual import * +import serial +import string +import math +import sys + +from time import time + +#ev = scene.waitfor('click keydown') + +grad2rad = 3.141592/180.0 + +# Check your COM port and baud rate +ser = serial.Serial(port='/dev/ttyUSB0',baudrate=57600, timeout=1) + +# Main scene +scene=display(title="9DOF Razor IMU test") +scene.range=(1.2,1.2,1.2) +#scene.forward = (0,-1,-0.25) +scene.forward = (1,0,-0.25) +scene.up=(0,0,1) + +# Second scene (Roll, Pitch, Yaw) +scene2 = display(title='9DOF Razor IMU test',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0)) +scene2.range=(1,1,1) +scene.width=500 +scene.y=200 + +scene2.select() +#Roll, Pitch, Yaw +cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red) +cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red) +cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green) +cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green) +#cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue) +#cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue) +arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1) + +#Roll,Pitch,Yaw labels +label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0) +label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0) +label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0) +label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow) +label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow) +label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow) +label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow) +label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow) +label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow) +label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow) +label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow) + +L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0) +L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0) +L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0) + +# Main scene objects +scene.select() +# Reference axis (x,y,z) +arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1) +arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1) +arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1) +# labels +label(pos=(0,0,0.8),text="9DOF Razor IMU test",box=0,opacity=0) +label(pos=(1,0,0),text="X",box=0,opacity=0) +label(pos=(0,-1,0),text="Y",box=0,opacity=0) +label(pos=(0,0,-1),text="Z",box=0,opacity=0) +# IMU object +platform = box(length=1, height=0.05, width=1, color=color.red) +p_line = box(length=1,height=0.08,width=0.1,color=color.yellow) +plat_arrow = arrow(color=color.green,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1) + + +f = open("Serial"+str(time())+".txt", 'w') + +roll=0 +pitch=0 +yaw=0 +cpt = 0 +t_start = time() +while 1: + line = ser.readline() + line = line.replace("!ANG:","") # Delete "!ANG:" + #print line + f.write(line) # Write to the output log file + words = string.split(line,"\t") # Fields split + #print words + print words + if len(words) != 3: + continue + + """ + a = float(words[0])#*grad2rad + b = float(words[1])#*grad2rad + c = float(words[2])#*grad2rad + d = float(words[3])#*grad2rad + + platform.axis = (1, 1, 1) + #print a, b, c, d + platform.rotate(angle=a, axis = (b, c, d)) + continue + """ + try: + roll = float(words[0])#*grad2rad + pitch = float(words[1])#*grad2rad + yaw = float(words[2])#*grad2rad + except: + print "Invalid line" + continue + + cpt += 1 + if cpt > 10: + t = time() - t_start + t_start = time() + print "XXX", cpt/t + cpt = 0 + + axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) + 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)) + platform.axis=axis + platform.up=up + platform.length=1.0 + platform.width=0.65 + plat_arrow.axis=axis + plat_arrow.up=up + plat_arrow.length=0.8 + p_line.axis=axis + p_line.up=up + cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0) + cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0) + cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0) + cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0) + arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0) + L1.text = str(float(words[0])) + L2.text = str(float(words[1])) + L3.text = str(float(words[2])) +ser.close() +f.close() diff --git a/imuboard/MadgwickAHRS.c b/imuboard/MadgwickAHRS.c new file mode 100644 index 0000000..5f03d6f --- /dev/null +++ b/imuboard/MadgwickAHRS.c @@ -0,0 +1,239 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2011-2012, SOH Madgwick + * + * 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 3 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, see . + * + */ + +//============================================================================ +// MadgwickAHRS.c +//============================================================================ +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised +// +//============================================================================ + +#include "MadgwickAHRS.h" +#include + +//#define sampleFreq 512.0f // sample frequency in Hz +//#define sampleFreq 46.0f // sample frequency in Hz +#define sampleFreq 85.0f // sample frequency in Hz +#define betaDef 0.1f // 2 * proportional gain + +volatile float beta = betaDef; // 2 * proportional gain (Kp) +volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame + + +static float invSqrt(float x) +{ + return 1.0f / sqrtf(x); +} + +/* AHRS algorithm update */ +void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat) +{ + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz; + float _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3; + float q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; + float mx, my, mz, ax, ay, az; + float q0 = quat->q0; + float q1 = quat->q1; + float q2 = quat->q2; + float q3 = quat->q3; + + /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in + * magnetometer normalisation) */ + if ((imu->mx == 0.0f) && (imu->my == 0.0f) && (imu->mz == 0.0f)) { + MadgwickAHRSupdateIMU(imu, quat); + return; + } + + /* Rate of change of quaternion from gyroscope */ + qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); + qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); + qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); + qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN + * in accelerometer normalisation) */ + if (!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { + + /* Normalise accelerometer measurement */ + recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + + imu->az * imu->az); + ax = imu->ax * recipNorm; + ay = imu->ay * recipNorm; + az = imu->az * recipNorm; + + /* Normalise magnetometer measurement */ + recipNorm = invSqrt(imu->mx * imu->mx + imu->my * imu->my + + imu->mz * imu->mz); + mx = imu->mx * recipNorm; + my = imu->my * recipNorm; + mz = imu->mz * recipNorm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0mx = 2.0f * q0 * mx; + _2q0my = 2.0f * q0 * my; + _2q0mz = 2.0f * q0 * mz; + _2q1mx = 2.0f * q1 * mx; + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _2q0q2 = 2.0f * q0 * q2; + _2q2q3 = 2.0f * q2 * q3; + q0q0 = q0 * q0; + q0q1 = q0 * q1; + q0q2 = q0 * q2; + q0q3 = q0 * q3; + q1q1 = q1 * q1; + q1q2 = q1 * q2; + q1q3 = q1 * q3; + q2q2 = q2 * q2; + q2q3 = q2 * q3; + q3q3 = q3 * q3; + + /* Reference direction of Earth's magnetic field */ + hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3; + hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3; + _2bx = sqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + /* Gradient decent algorithm corrective step */ + 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); + 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); + 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); + 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); + + /* normalize step magnitude */ + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + /* Apply feedback step */ + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + q0 += qDot1 * (1.0f / sampleFreq); + q1 += qDot2 * (1.0f / sampleFreq); + q2 += qDot3 * (1.0f / sampleFreq); + q3 += qDot4 * (1.0f / sampleFreq); + + /* Normalise quaternion */ + recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); + quat->q0 = q0 * recipNorm; + quat->q1 = q1 * recipNorm; + quat->q2 = q2 * recipNorm; + quat->q3 = q3 * recipNorm; +} + +/* IMU algorithm update (does not take magneto in account) */ +void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat) +{ + float recipNorm; + float s0, s1, s2, s3; + float qDot1, qDot2, qDot3, qDot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3; + float ax, ay, az; + float q0 = quat->q0; + float q1 = quat->q1; + float q2 = quat->q2; + float q3 = quat->q3; + + /* Rate of change of quaternion from gyroscope */ + qDot1 = 0.5f * (-q1 * imu->gx - q2 * imu->gy - q3 * imu->gz); + qDot2 = 0.5f * (q0 * imu->gx + q2 * imu->gz - q3 * imu->gy); + qDot3 = 0.5f * (q0 * imu->gy - q1 * imu->gz + q3 * imu->gx); + qDot4 = 0.5f * (q0 * imu->gz + q1 * imu->gy - q2 * imu->gx); + + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) */ + if(!((imu->ax == 0.0f) && (imu->ay == 0.0f) && (imu->az == 0.0f))) { + + /* Normalise accelerometer measurement */ + recipNorm = invSqrt(imu->ax * imu->ax + imu->ay * imu->ay + imu->az * imu->az); + ax = imu->ax * recipNorm; + ay = imu->ay * recipNorm; + az = imu->az * recipNorm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0 = 2.0f * q0; + _2q1 = 2.0f * q1; + _2q2 = 2.0f * q2; + _2q3 = 2.0f * q3; + _4q0 = 4.0f * q0; + _4q1 = 4.0f * q1; + _4q2 = 4.0f * q2; + _8q1 = 8.0f * q1; + _8q2 = 8.0f * q2; + q0q0 = q0 * q0; + q1q1 = q1 * q1; + q2q2 = q2 * q2; + q3q3 = q3 * q3; + + /* Gradient decent algorithm corrective step */ + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; + recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); /* normalise step magnitude */ + + s0 *= recipNorm; + s1 *= recipNorm; + s2 *= recipNorm; + s3 *= recipNorm; + + /* Apply feedback step */ + qDot1 -= beta * s0; + qDot2 -= beta * s1; + qDot3 -= beta * s2; + qDot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + q0 += qDot1 * (1.0f / sampleFreq); + q1 += qDot2 * (1.0f / sampleFreq); + q2 += qDot3 * (1.0f / sampleFreq); + q3 += qDot4 * (1.0f / sampleFreq); + + /* Normalise quaternion */ + recipNorm = invSqrt(q0 * q0 + q1 * q1 + + q2 * q2 + q3 * q3); + quat->q0 = q0 * recipNorm; + quat->q1 = q1 * recipNorm; + quat->q2 = q2 * recipNorm; + quat->q3 = q3 * recipNorm; +} + diff --git a/imuboard/MadgwickAHRS.h b/imuboard/MadgwickAHRS.h new file mode 100644 index 0000000..018f34a --- /dev/null +++ b/imuboard/MadgwickAHRS.h @@ -0,0 +1,46 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2011, SOH Madgwick + * + * 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 3 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, see . + * + */ + +//============================================================================= +// MadgwickAHRS.h +//============================================================================= +// +// Implementation of Madgwick's IMU and AHRS algorithms. +// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// +//============================================================================= + +#ifndef MadgwickAHRS_h +#define MadgwickAHRS_h + +#include "imu.h" + +extern volatile float beta; // algorithm gain + +/* update quaternion structure using the new IMU infos */ +void MadgwickAHRSupdate(const struct imu_info *imu, struct quaternion *quat); + +/* update quaternion structure using the new IMU infos, without using magneto */ +void MadgwickAHRSupdateIMU(const struct imu_info *imu, struct quaternion *quat); + +#endif diff --git a/imuboard/Makefile b/imuboard/Makefile new file mode 100644 index 0000000..4ab55ad --- /dev/null +++ b/imuboard/Makefile @@ -0,0 +1,30 @@ +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 diff --git a/imuboard/PS-MPU-9150A.pdf b/imuboard/PS-MPU-9150A.pdf new file mode 100644 index 0000000..309f5c8 Binary files /dev/null and b/imuboard/PS-MPU-9150A.pdf differ diff --git a/imuboard/RM-MPU-9150A-00.pdf b/imuboard/RM-MPU-9150A-00.pdf new file mode 100644 index 0000000..4227dea Binary files /dev/null and b/imuboard/RM-MPU-9150A-00.pdf differ diff --git a/imuboard/byteordering.c b/imuboard/byteordering.c new file mode 100644 index 0000000..46ff7d8 --- /dev/null +++ b/imuboard/byteordering.c @@ -0,0 +1,110 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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); +} + +/** + * @} + */ + diff --git a/imuboard/byteordering.h b/imuboard/byteordering.h new file mode 100644 index 0000000..824c70f --- /dev/null +++ b/imuboard/byteordering.h @@ -0,0 +1,188 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 + +#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 + diff --git a/imuboard/cmdline.c b/imuboard/cmdline.c new file mode 100644 index 0000000..91360a9 --- /dev/null +++ b/imuboard/cmdline.c @@ -0,0 +1,236 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include + +#include +#include +#include + +#include +#include +#include + +#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; ierr_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); +} diff --git a/imuboard/cmdline.h b/imuboard/cmdline.h new file mode 100644 index 0000000..034244a --- /dev/null +++ b/imuboard/cmdline.h @@ -0,0 +1,63 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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_ */ diff --git a/imuboard/commands.c b/imuboard/commands.c new file mode 100644 index 0000000..42d243f --- /dev/null +++ b/imuboard/commands.c @@ -0,0 +1,276 @@ +/* + * 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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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, +}; diff --git a/imuboard/commands_gen.c b/imuboard/commands_gen.c new file mode 100644 index 0000000..f49ef9a --- /dev/null +++ b/imuboard/commands_gen.c @@ -0,0 +1,382 @@ +/* + * 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 + */ + +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#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; iarg1, 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; iarg3, PSTR("off"))) { + for (i=0; i + * + * 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 +#include +#include + +#include +#include +#include + +#include + + +#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; +} diff --git a/imuboard/eeprom_config.h b/imuboard/eeprom_config.h new file mode 100644 index 0000000..7aff867 --- /dev/null +++ b/imuboard/eeprom_config.h @@ -0,0 +1,48 @@ +/* + * Copyright 2013 Olivier Matz + * + * 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 diff --git a/imuboard/error_config.h b/imuboard/error_config.h new file mode 100644 index 0000000..08c3de7 --- /dev/null +++ b/imuboard/error_config.h @@ -0,0 +1,28 @@ +/* + * 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 diff --git a/imuboard/fat.c b/imuboard/fat.c new file mode 100644 index 0000000..df579ce --- /dev/null +++ b/imuboard/fat.c @@ -0,0 +1,2558 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 + +#if USE_DYNAMIC_MEMORY + #include +#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 + diff --git a/imuboard/fat.h b/imuboard/fat.h new file mode 100644 index 0000000..b67bb29 --- /dev/null +++ b/imuboard/fat.h @@ -0,0 +1,131 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 +#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 + diff --git a/imuboard/fat_config.h b/imuboard/fat_config.h new file mode 100644 index 0000000..6e22e00 --- /dev/null +++ b/imuboard/fat_config.h @@ -0,0 +1,128 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 +#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 + diff --git a/imuboard/gps_venus.c b/imuboard/gps_venus.c new file mode 100644 index 0000000..22e2076 --- /dev/null +++ b/imuboard/gps_venus.c @@ -0,0 +1,600 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#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; +} diff --git a/imuboard/gps_venus.h b/imuboard/gps_venus.h new file mode 100644 index 0000000..ca2bd8f --- /dev/null +++ b/imuboard/gps_venus.h @@ -0,0 +1,52 @@ +#ifndef _GPS_VENUS_H +#define _GPS_VENUS_H + +#include + +/* 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 diff --git a/imuboard/graph_imu.py b/imuboard/graph_imu.py new file mode 100644 index 0000000..e32716d --- /dev/null +++ b/imuboard/graph_imu.py @@ -0,0 +1,83 @@ +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() + + diff --git a/imuboard/i2c_config.h b/imuboard/i2c_config.h new file mode 100644 index 0000000..93ce0ec --- /dev/null +++ b/imuboard/i2c_config.h @@ -0,0 +1,30 @@ +/* + * 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 diff --git a/imuboard/i2c_protocol.c b/imuboard/i2c_protocol.c new file mode 100644 index 0000000..5c86af0 --- /dev/null +++ b/imuboard/i2c_protocol.c @@ -0,0 +1,157 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * 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 + +#include +#include + +#include + +#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) +{ +} + + diff --git a/imuboard/i2c_protocol.h b/imuboard/i2c_protocol.h new file mode 100644 index 0000000..7f022ef --- /dev/null +++ b/imuboard/i2c_protocol.h @@ -0,0 +1,30 @@ +/* + * 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 + +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); diff --git a/imuboard/i2cm_sw.c b/imuboard/i2cm_sw.c new file mode 100644 index 0000000..a7da2b5 --- /dev/null +++ b/imuboard/i2cm_sw.c @@ -0,0 +1,332 @@ +/* + * 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 +#include +#include +#include + +#include + + +#include + + +#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>(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 diff --git a/imuboard/imu.c b/imuboard/imu.c new file mode 100644 index 0000000..7e9a17f --- /dev/null +++ b/imuboard/imu.c @@ -0,0 +1,133 @@ +#include +#include +#include + +#include +#include +#include + +#include + +#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; +} diff --git a/imuboard/imu.h b/imuboard/imu.h new file mode 100644 index 0000000..06c03a1 --- /dev/null +++ b/imuboard/imu.h @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2014, Fabrice DESCLAUX + * + * 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 diff --git a/imuboard/main.c b/imuboard/main.c new file mode 100644 index 0000000..386528b --- /dev/null +++ b/imuboard/main.c @@ -0,0 +1,196 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#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; +} diff --git a/imuboard/main.h b/imuboard/main.h new file mode 100644 index 0000000..ad65316 --- /dev/null +++ b/imuboard/main.h @@ -0,0 +1,87 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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_ */ diff --git a/imuboard/matrix.c b/imuboard/matrix.c new file mode 100644 index 0000000..898aa82 --- /dev/null +++ b/imuboard/matrix.c @@ -0,0 +1,16 @@ +//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]; + } + } +} diff --git a/imuboard/matrix.h b/imuboard/matrix.h new file mode 100644 index 0000000..6258f1c --- /dev/null +++ b/imuboard/matrix.h @@ -0,0 +1,6 @@ +#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_ diff --git a/imuboard/mpu6050.c b/imuboard/mpu6050.c new file mode 100644 index 0000000..2966ee4 --- /dev/null +++ b/imuboard/mpu6050.c @@ -0,0 +1,351 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2014, Fabrice DESCLAUX + * + * 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 +#include + +#include + +#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; +} diff --git a/imuboard/mpu6050.h b/imuboard/mpu6050.h new file mode 100644 index 0000000..3fabfa9 --- /dev/null +++ b/imuboard/mpu6050.h @@ -0,0 +1,43 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * Copyright (c) 2014, Fabrice DESCLAUX + * + * 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_ */ diff --git a/imuboard/mpu6050_regs.h b/imuboard/mpu6050_regs.h new file mode 100644 index 0000000..83b7e3a --- /dev/null +++ b/imuboard/mpu6050_regs.h @@ -0,0 +1,116 @@ + +#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_ diff --git a/imuboard/partition.c b/imuboard/partition.c new file mode 100644 index 0000000..d4eb015 --- /dev/null +++ b/imuboard/partition.c @@ -0,0 +1,155 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 + +#if USE_DYNAMIC_MEMORY + #include +#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; +} + +/** + * @} + */ + diff --git a/imuboard/partition.h b/imuboard/partition.h new file mode 100644 index 0000000..d559c6e --- /dev/null +++ b/imuboard/partition.h @@ -0,0 +1,212 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 +#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 + diff --git a/imuboard/partition_config.h b/imuboard/partition_config.h new file mode 100644 index 0000000..0b49138 --- /dev/null +++ b/imuboard/partition_config.h @@ -0,0 +1,44 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 + diff --git a/imuboard/rdline_config.h b/imuboard/rdline_config.h new file mode 100644 index 0000000..e69de29 diff --git a/imuboard/scheduler_config.h b/imuboard/scheduler_config.h new file mode 100644 index 0000000..708bfd6 --- /dev/null +++ b/imuboard/scheduler_config.h @@ -0,0 +1,77 @@ +/* + * 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_ diff --git a/imuboard/sd-reader_config.h b/imuboard/sd-reader_config.h new file mode 100644 index 0000000..16957f8 --- /dev/null +++ b/imuboard/sd-reader_config.h @@ -0,0 +1,53 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 + diff --git a/imuboard/sd_log.c b/imuboard/sd_log.c new file mode 100644 index 0000000..97496f4 --- /dev/null +++ b/imuboard/sd_log.c @@ -0,0 +1,146 @@ +#include +#include +#include +#include + +#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; +} diff --git a/imuboard/sd_log.h b/imuboard/sd_log.h new file mode 100644 index 0000000..d386126 --- /dev/null +++ b/imuboard/sd_log.h @@ -0,0 +1,30 @@ +#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 diff --git a/imuboard/sd_main.c b/imuboard/sd_main.c new file mode 100644 index 0000000..06bd231 --- /dev/null +++ b/imuboard/sd_main.c @@ -0,0 +1,656 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 +#include +#include +#include "fat.h" +#include "fat_config.h" +#include "partition.h" +#include "sd_raw.h" +#include "sd_raw_config.h" + +#include +#include +#include +#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 + * project homepage 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 + * benchmark page 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. + * - cat \\n + * Writes a hexdump of \ to the terminal. + * - cd \\n + * Changes current working directory to \. + * - disk\n + * Shows card manufacturer, status, filesystem capacity and free storage space. + * - init\n + * Reinitializes and reopens the memory card. + * - ls\n + * Shows the content of the current directory. + * - mkdir \\n + * Creates a directory called \. + * - mv \ \\n + * Renames \ to \. + * - rm \\n + * Deletes \. + * - sync\n + * Ensures all buffered data is written to the card. + * - touch \\n + * Creates \. + * - write \ \\n + * Writes text to \, starting from \. The text is read + * from the UART, line by line. Finish with an empty line. + * + * \htmlonly + *

+ * 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: + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
layercode sizestatic RAM usage
MMC/SD2410518
Partition45617
FAT167928188
+ * + *

+ * The static RAM is mostly used for buffering memory card access, which + * improves performance and reduces implementation complexity. + *

+ * + *

+ * 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. + *

+ * + *

+ * 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. + *

+ * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + * + *
descriptordynamic/static RAM
partition17
filesystem26
file53
directory49
+ * + * \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 + * FAQ page. + * + * \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 + + diff --git a/imuboard/sd_raw.c b/imuboard/sd_raw.c new file mode 100644 index 0000000..f33184e --- /dev/null +++ b/imuboard/sd_raw.c @@ -0,0 +1,1002 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 +#include +#include +#include "sd_raw.h" + +#include + +/** + * \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; +} + diff --git a/imuboard/sd_raw.h b/imuboard/sd_raw.h new file mode 100644 index 0000000..e4ab587 --- /dev/null +++ b/imuboard/sd_raw.h @@ -0,0 +1,148 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 +#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 + diff --git a/imuboard/sd_raw_config.h b/imuboard/sd_raw_config.h new file mode 100644 index 0000000..777d8d1 --- /dev/null +++ b/imuboard/sd_raw_config.h @@ -0,0 +1,151 @@ + +/* + * Copyright (c) 2006-2012 by Roland Riegel + * + * 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 + +#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 + diff --git a/imuboard/timer_config.h b/imuboard/timer_config.h new file mode 100644 index 0000000..47d9f18 --- /dev/null +++ b/imuboard/timer_config.h @@ -0,0 +1,36 @@ +/* + * 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 diff --git a/imuboard/translate_code.py b/imuboard/translate_code.py new file mode 100644 index 0000000..715a290 --- /dev/null +++ b/imuboard/translate_code.py @@ -0,0 +1,90 @@ +#! /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 + diff --git a/imuboard/uart_config.h b/imuboard/uart_config.h new file mode 100644 index 0000000..3785e57 --- /dev/null +++ b/imuboard/uart_config.h @@ -0,0 +1,92 @@ +/* + * 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: uart_config.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART0 definitions + */ + +/* compile uart0 fonctions, undefine it to pass compilation */ +#define UART0_COMPILE + +/* enable uart0 if == 1, disable if == 0 */ +#define UART0_ENABLED 1 + +/* enable uart0 interrupts if == 1, disable if == 0 */ +#define UART0_INTERRUPT_ENABLED 1 + +#define UART0_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART0_USE_DOUBLE_SPEED 1 + +#define UART0_RX_FIFO_SIZE 64 +#define UART0_TX_FIFO_SIZE 127 +#define UART0_NBITS 8 + +#define UART0_PARITY UART_PARTITY_NONE + +#define UART0_STOP_BIT UART_STOP_BITS_1 + +/* .... same for uart 1, 2, 3 ... */ + +/* + * UART1 definitions + */ + +/* compile uart1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable uart1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable uart1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 1 + +#define UART1_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART1_USE_DOUBLE_SPEED 1 + +#define UART1_RX_FIFO_SIZE 64 +#define UART1_TX_FIFO_SIZE 127 +#define UART1_NBITS 8 + +#define UART1_PARITY UART_PARTITY_NONE + +#define UART1_STOP_BIT UART_STOP_BITS_1 + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/mainboard/Makefile b/mainboard/Makefile new file mode 100644 index 0000000..7fcda32 --- /dev/null +++ b/mainboard/Makefile @@ -0,0 +1,26 @@ +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 diff --git a/mainboard/avr6.x b/mainboard/avr6.x new file mode 100644 index 0000000..139b117 --- /dev/null +++ b/mainboard/avr6.x @@ -0,0 +1,251 @@ +/* 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) } +} diff --git a/mainboard/beep.c b/mainboard/beep.c new file mode 100644 index 0000000..1a31186 --- /dev/null +++ b/mainboard/beep.c @@ -0,0 +1,109 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 +#include + +#include +#include + +#include + +#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); +} diff --git a/mainboard/beep.h b/mainboard/beep.h new file mode 100644 index 0000000..50e5750 --- /dev/null +++ b/mainboard/beep.h @@ -0,0 +1,32 @@ +/* + * Copyright (c) 2013, Olivier MATZ + * 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); diff --git a/mainboard/cmdline.c b/mainboard/cmdline.c new file mode 100644 index 0000000..568dcad --- /dev/null +++ b/mainboard/cmdline.c @@ -0,0 +1,236 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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 +#include + +#include +#include +#include + +#include +#include +#include + +#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; ierr_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); +} diff --git a/mainboard/cmdline.h b/mainboard/cmdline.h new file mode 100644 index 0000000..034244a --- /dev/null +++ b/mainboard/cmdline.h @@ -0,0 +1,63 @@ +/* + * Copyright Droids Corporation + * Olivier Matz + * + * 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_ */ diff --git a/mainboard/commands.c b/mainboard/commands.c new file mode 100644 index 0000000..a7b8e87 --- /dev/null +++ b/mainboard/commands.c @@ -0,0 +1,2356 @@ +/* + * 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 + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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, +}; diff --git a/mainboard/commands_gen.c b/mainboard/commands_gen.c new file mode 100644 index 0000000..8f962e3 --- /dev/null +++ b/mainboard/commands_gen.c @@ -0,0 +1,382 @@ +/* + * 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 + */ + +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +#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; iarg1, 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; iarg3, PSTR("off"))) { + for (i=0; i logs input and output to + log logs input to and output to + 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 diff --git a/mainboard/eeprom_config.c b/mainboard/eeprom_config.c new file mode 100644 index 0000000..f3274d8 --- /dev/null +++ b/mainboard/eeprom_config.c @@ -0,0 +1,157 @@ +/* + * Copyright 2013 Olivier Matz + * + * 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 +#include +#include + +#include +#include +#include + +#include + + +#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; +} diff --git a/mainboard/eeprom_config.h b/mainboard/eeprom_config.h new file mode 100644 index 0000000..7aff867 --- /dev/null +++ b/mainboard/eeprom_config.h @@ -0,0 +1,48 @@ +/* + * Copyright 2013 Olivier Matz + * + * 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 diff --git a/mainboard/error_config.h b/mainboard/error_config.h new file mode 100644 index 0000000..6433dde --- /dev/null +++ b/mainboard/error_config.h @@ -0,0 +1,31 @@ +/* + * 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 diff --git a/mainboard/i2c_config.h b/mainboard/i2c_config.h new file mode 100644 index 0000000..1617810 --- /dev/null +++ b/mainboard/i2c_config.h @@ -0,0 +1,30 @@ +/* + * 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 diff --git a/mainboard/i2c_protocol.c b/mainboard/i2c_protocol.c new file mode 100644 index 0000000..ce3d262 --- /dev/null +++ b/mainboard/i2c_protocol.c @@ -0,0 +1,341 @@ +/* + * 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 +#include + +#include +#include +#include + +#include +#include + +#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); +} diff --git a/mainboard/i2c_protocol.h b/mainboard/i2c_protocol.h new file mode 100644 index 0000000..66d5831 --- /dev/null +++ b/mainboard/i2c_protocol.h @@ -0,0 +1,45 @@ +/* + * 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 diff --git a/mainboard/main.c b/mainboard/main.c new file mode 100644 index 0000000..0d46bf2 --- /dev/null +++ b/mainboard/main.c @@ -0,0 +1,213 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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; +} diff --git a/mainboard/main.h b/mainboard/main.h new file mode 100644 index 0000000..7ee9ed7 --- /dev/null +++ b/mainboard/main.h @@ -0,0 +1,93 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#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); diff --git a/mainboard/notes.txt b/mainboard/notes.txt new file mode 100644 index 0000000..882dbc3 --- /dev/null +++ b/mainboard/notes.txt @@ -0,0 +1,503 @@ +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. diff --git a/mainboard/parse_atcmd.c b/mainboard/parse_atcmd.c new file mode 100644 index 0000000..fbb0c6e --- /dev/null +++ b/mainboard/parse_atcmd.c @@ -0,0 +1,134 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 + +#include +#include +#include +#include + +#include +#include + +#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, +}; diff --git a/mainboard/parse_atcmd.h b/mainboard/parse_atcmd.h new file mode 100644 index 0000000..8506053 --- /dev/null +++ b/mainboard/parse_atcmd.h @@ -0,0 +1,64 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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_ */ diff --git a/mainboard/parse_monitor.c b/mainboard/parse_monitor.c new file mode 100644 index 0000000..1b5bed2 --- /dev/null +++ b/mainboard/parse_monitor.c @@ -0,0 +1,123 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 +#include +#include + +#include +#include +#include +#include + +#include + +#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, +}; diff --git a/mainboard/parse_monitor.h b/mainboard/parse_monitor.h new file mode 100644 index 0000000..8519c19 --- /dev/null +++ b/mainboard/parse_monitor.h @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 + +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_ */ diff --git a/mainboard/parse_neighbor.c b/mainboard/parse_neighbor.c new file mode 100644 index 0000000..d04092b --- /dev/null +++ b/mainboard/parse_neighbor.c @@ -0,0 +1,132 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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 +#include +#include + +#include +#include +#include +#include + +#include +#include + +#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, +}; diff --git a/mainboard/parse_neighbor.h b/mainboard/parse_neighbor.h new file mode 100644 index 0000000..6d4cfcd --- /dev/null +++ b/mainboard/parse_neighbor.h @@ -0,0 +1,60 @@ +/* + * Copyright (c) 2011, Olivier MATZ + * 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_ */ diff --git a/mainboard/rc_proto.c b/mainboard/rc_proto.c new file mode 100644 index 0000000..1c1a29f --- /dev/null +++ b/mainboard/rc_proto.c @@ -0,0 +1,851 @@ +/* + * Copyright (c) 2013, Olivier MATZ + * 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 + +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#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); +} diff --git a/mainboard/rc_proto.h b/mainboard/rc_proto.h new file mode 100644 index 0000000..baf90f4 --- /dev/null +++ b/mainboard/rc_proto.h @@ -0,0 +1,151 @@ +#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 diff --git a/mainboard/rdline_config.h b/mainboard/rdline_config.h new file mode 100644 index 0000000..e69de29 diff --git a/mainboard/scheduler_config.h b/mainboard/scheduler_config.h new file mode 100644 index 0000000..d4cbc68 --- /dev/null +++ b/mainboard/scheduler_config.h @@ -0,0 +1,53 @@ +/* + * 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_ diff --git a/mainboard/spi_config.h b/mainboard/spi_config.h new file mode 100644 index 0000000..76697c3 --- /dev/null +++ b/mainboard/spi_config.h @@ -0,0 +1,36 @@ +/* + * 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 + diff --git a/mainboard/spi_servo.c b/mainboard/spi_servo.c new file mode 100644 index 0000000..7a85070 --- /dev/null +++ b/mainboard/spi_servo.c @@ -0,0 +1,272 @@ +#include +#include + +#include +#include + +#include + +#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<> 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<= 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()); +} diff --git a/mainboard/spi_servo.h b/mainboard/spi_servo.h new file mode 100644 index 0000000..95449c5 --- /dev/null +++ b/mainboard/spi_servo.h @@ -0,0 +1,10 @@ +#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); diff --git a/mainboard/time_config.h b/mainboard/time_config.h new file mode 100644 index 0000000..14db608 --- /dev/null +++ b/mainboard/time_config.h @@ -0,0 +1,23 @@ +/* + * 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 diff --git a/mainboard/timer_config.h b/mainboard/timer_config.h new file mode 100644 index 0000000..47d9f18 --- /dev/null +++ b/mainboard/timer_config.h @@ -0,0 +1,36 @@ +/* + * 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 diff --git a/mainboard/uart_config.h b/mainboard/uart_config.h new file mode 100644 index 0000000..3785e57 --- /dev/null +++ b/mainboard/uart_config.h @@ -0,0 +1,92 @@ +/* + * 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: uart_config.h,v 1.5 2009-11-08 17:24:33 zer0 Exp $ + * + */ + +/* Droids-corp 2004 - Zer0 + * config for uart module + */ + +#ifndef UART_CONFIG_H +#define UART_CONFIG_H + +/* + * UART0 definitions + */ + +/* compile uart0 fonctions, undefine it to pass compilation */ +#define UART0_COMPILE + +/* enable uart0 if == 1, disable if == 0 */ +#define UART0_ENABLED 1 + +/* enable uart0 interrupts if == 1, disable if == 0 */ +#define UART0_INTERRUPT_ENABLED 1 + +#define UART0_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART0_USE_DOUBLE_SPEED 1 + +#define UART0_RX_FIFO_SIZE 64 +#define UART0_TX_FIFO_SIZE 127 +#define UART0_NBITS 8 + +#define UART0_PARITY UART_PARTITY_NONE + +#define UART0_STOP_BIT UART_STOP_BITS_1 + +/* .... same for uart 1, 2, 3 ... */ + +/* + * UART1 definitions + */ + +/* compile uart1 fonctions, undefine it to pass compilation */ +#define UART1_COMPILE + +/* enable uart1 if == 1, disable if == 0 */ +#define UART1_ENABLED 1 + +/* enable uart1 interrupts if == 1, disable if == 0 */ +#define UART1_INTERRUPT_ENABLED 1 + +#define UART1_BAUDRATE 57600 + +/* + * if you enable this, the maximum baudrate you can reach is + * higher, but the precision is lower. + */ +#define UART1_USE_DOUBLE_SPEED 1 + +#define UART1_RX_FIFO_SIZE 64 +#define UART1_TX_FIFO_SIZE 127 +#define UART1_NBITS 8 + +#define UART1_PARITY UART_PARTITY_NONE + +#define UART1_STOP_BIT UART_STOP_BITS_1 + +/* .... same for uart 1, 2, 3 ... */ + +#endif + diff --git a/mainboard/xbee_user.c b/mainboard/xbee_user.c new file mode 100644 index 0000000..17a92af --- /dev/null +++ b/mainboard/xbee_user.c @@ -0,0 +1,569 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * 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 +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#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); +} diff --git a/mainboard/xbee_user.h b/mainboard/xbee_user.h new file mode 100644 index 0000000..bba7885 --- /dev/null +++ b/mainboard/xbee_user.h @@ -0,0 +1,92 @@ +/* + * Copyright (c) 2014, Olivier MATZ + * 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 +#include + +#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_ */