mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 01:03:33 +08:00
Merge branch 'navigator_new' into navigator_new_vector
This commit is contained in:
@@ -1,265 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. 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.
|
||||
* 3. Neither the name PX4 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 COPYRIGHT HOLDERS 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
|
||||
* COPYRIGHT OWNER OR 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file eeprom.c
|
||||
*
|
||||
* EEPROM service and utility app.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/mount.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include <nuttx/i2c.h>
|
||||
#include <nuttx/mtd.h>
|
||||
#include <nuttx/fs/nxffs.h>
|
||||
#include <nuttx/fs/ioctl.h>
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
#include "systemlib/systemlib.h"
|
||||
#include "systemlib/param/param.h"
|
||||
#include "systemlib/err.h"
|
||||
|
||||
#ifndef PX4_I2C_BUS_ONBOARD
|
||||
# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
|
||||
#endif
|
||||
|
||||
__EXPORT int eeprom_main(int argc, char *argv[]);
|
||||
|
||||
static void eeprom_attach(void);
|
||||
static void eeprom_start(void);
|
||||
static void eeprom_erase(void);
|
||||
static void eeprom_ioctl(unsigned operation);
|
||||
static void eeprom_save(const char *name);
|
||||
static void eeprom_load(const char *name);
|
||||
static void eeprom_test(void);
|
||||
|
||||
static bool attached = false;
|
||||
static bool started = false;
|
||||
static struct mtd_dev_s *eeprom_mtd;
|
||||
|
||||
int eeprom_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 2) {
|
||||
if (!strcmp(argv[1], "start"))
|
||||
eeprom_start();
|
||||
|
||||
if (!strcmp(argv[1], "save_param"))
|
||||
eeprom_save(argv[2]);
|
||||
|
||||
if (!strcmp(argv[1], "load_param"))
|
||||
eeprom_load(argv[2]);
|
||||
|
||||
if (!strcmp(argv[1], "erase"))
|
||||
eeprom_erase();
|
||||
|
||||
if (!strcmp(argv[1], "test"))
|
||||
eeprom_test();
|
||||
|
||||
if (0) { /* these actually require a file on the filesystem... */
|
||||
|
||||
if (!strcmp(argv[1], "reformat"))
|
||||
eeprom_ioctl(FIOC_REFORMAT);
|
||||
|
||||
if (!strcmp(argv[1], "repack"))
|
||||
eeprom_ioctl(FIOC_OPTIMIZE);
|
||||
}
|
||||
}
|
||||
|
||||
errx(1, "expected a command, try 'start'\n\t'save_param /eeprom/parameters'\n\t'load_param /eeprom/parameters'\n\t'erase'\n");
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
eeprom_attach(void)
|
||||
{
|
||||
/* find the right I2C */
|
||||
struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
|
||||
/* this resets the I2C bus, set correct bus speed again */
|
||||
I2C_SETFREQUENCY(i2c, 400000);
|
||||
|
||||
if (i2c == NULL)
|
||||
errx(1, "failed to locate I2C bus");
|
||||
|
||||
/* start the MTD driver, attempt 5 times */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
eeprom_mtd = at24c_initialize(i2c);
|
||||
if (eeprom_mtd) {
|
||||
/* abort on first valid result */
|
||||
if (i > 0) {
|
||||
warnx("warning: EEPROM needed %d attempts to attach", i+1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* if last attempt is still unsuccessful, abort */
|
||||
if (eeprom_mtd == NULL)
|
||||
errx(1, "failed to initialize EEPROM driver");
|
||||
|
||||
attached = true;
|
||||
}
|
||||
|
||||
static void
|
||||
eeprom_start(void)
|
||||
{
|
||||
int ret;
|
||||
|
||||
if (started)
|
||||
errx(1, "EEPROM already mounted");
|
||||
|
||||
if (!attached)
|
||||
eeprom_attach();
|
||||
|
||||
/* start NXFFS */
|
||||
ret = nxffs_initialize(eeprom_mtd);
|
||||
|
||||
if (ret < 0)
|
||||
errx(1, "failed to initialize NXFFS - erase EEPROM to reformat");
|
||||
|
||||
/* mount the EEPROM */
|
||||
ret = mount(NULL, "/eeprom", "nxffs", 0, NULL);
|
||||
|
||||
if (ret < 0)
|
||||
errx(1, "failed to mount /eeprom - erase EEPROM to reformat");
|
||||
|
||||
started = true;
|
||||
warnx("mounted EEPROM at /eeprom");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
extern int at24c_nuke(void);
|
||||
|
||||
static void
|
||||
eeprom_erase(void)
|
||||
{
|
||||
if (!attached)
|
||||
eeprom_attach();
|
||||
|
||||
if (at24c_nuke())
|
||||
errx(1, "erase failed");
|
||||
|
||||
errx(0, "erase done, reboot now");
|
||||
}
|
||||
|
||||
static void
|
||||
eeprom_ioctl(unsigned operation)
|
||||
{
|
||||
int fd;
|
||||
|
||||
fd = open("/eeprom/.", 0);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "open /eeprom");
|
||||
|
||||
if (ioctl(fd, operation, 0) < 0)
|
||||
err(1, "ioctl");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
eeprom_save(const char *name)
|
||||
{
|
||||
if (!started)
|
||||
errx(1, "must be started first");
|
||||
|
||||
if (!name)
|
||||
err(1, "missing argument for device name, try '/eeprom/parameters'");
|
||||
|
||||
warnx("WARNING: 'eeprom save_param' deprecated - use 'param save' instead");
|
||||
|
||||
/* delete the file in case it exists */
|
||||
unlink(name);
|
||||
|
||||
/* create the file */
|
||||
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "opening '%s' failed", name);
|
||||
|
||||
int result = param_export(fd, false);
|
||||
close(fd);
|
||||
|
||||
if (result < 0) {
|
||||
unlink(name);
|
||||
errx(1, "error exporting to '%s'", name);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
eeprom_load(const char *name)
|
||||
{
|
||||
if (!started)
|
||||
errx(1, "must be started first");
|
||||
|
||||
if (!name)
|
||||
err(1, "missing argument for device name, try '/eeprom/parameters'");
|
||||
|
||||
warnx("WARNING: 'eeprom load_param' deprecated - use 'param load' instead");
|
||||
|
||||
int fd = open(name, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "open '%s'", name);
|
||||
|
||||
int result = param_load(fd);
|
||||
close(fd);
|
||||
|
||||
if (result < 0)
|
||||
errx(1, "error importing from '%s'", name);
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
extern void at24c_test(void);
|
||||
|
||||
static void
|
||||
eeprom_test(void)
|
||||
{
|
||||
at24c_test();
|
||||
exit(0);
|
||||
}
|
||||
@@ -1,39 +0,0 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. 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.
|
||||
# 3. Neither the name PX4 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 COPYRIGHT HOLDERS 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
|
||||
# COPYRIGHT OWNER OR 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# EEPROM file system driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = eeprom
|
||||
SRCS = 24xxxx_mtd.c eeprom.c
|
||||
@@ -3,4 +3,4 @@
|
||||
#
|
||||
|
||||
MODULE_COMMAND = mtd
|
||||
SRCS = mtd.c
|
||||
SRCS = mtd.c 24xxxx_mtd.c
|
||||
|
||||
+258
-44
@@ -62,26 +62,43 @@
|
||||
#include "systemlib/param/param.h"
|
||||
#include "systemlib/err.h"
|
||||
|
||||
#include <board_config.h>
|
||||
|
||||
__EXPORT int mtd_main(int argc, char *argv[]);
|
||||
|
||||
#ifndef CONFIG_MTD_RAMTRON
|
||||
#ifndef CONFIG_MTD
|
||||
|
||||
/* create a fake command with decent warnx to not confuse users */
|
||||
int mtd_main(int argc, char *argv[])
|
||||
{
|
||||
errx(1, "RAMTRON not enabled, skipping.");
|
||||
errx(1, "MTD not enabled, skipping.");
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
static void mtd_attach(void);
|
||||
#ifdef CONFIG_MTD_RAMTRON
|
||||
static void ramtron_attach(void);
|
||||
#else
|
||||
|
||||
#ifndef PX4_I2C_BUS_ONBOARD
|
||||
# error PX4_I2C_BUS_ONBOARD not defined, cannot locate onboard EEPROM
|
||||
#endif
|
||||
|
||||
static void at24xxx_attach(void);
|
||||
#endif
|
||||
static void mtd_start(char *partition_names[], unsigned n_partitions);
|
||||
static void mtd_test(void);
|
||||
static void mtd_erase(char *partition_names[], unsigned n_partitions);
|
||||
static void mtd_readtest(char *partition_names[], unsigned n_partitions);
|
||||
static void mtd_rwtest(char *partition_names[], unsigned n_partitions);
|
||||
static void mtd_print_info();
|
||||
static int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
|
||||
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions);
|
||||
|
||||
static bool attached = false;
|
||||
static bool started = false;
|
||||
static struct mtd_dev_s *mtd_dev;
|
||||
static unsigned n_partitions_current = 0;
|
||||
|
||||
/* note, these will be equally sized */
|
||||
static char *partition_names_default[] = {"/fs/mtd_params", "/fs/mtd_waypoints"};
|
||||
@@ -93,9 +110,8 @@ int mtd_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1], "start")) {
|
||||
|
||||
/* start mapping according to user request */
|
||||
if (argc > 3) {
|
||||
if (argc >= 3) {
|
||||
mtd_start(argv + 2, argc - 2);
|
||||
|
||||
} else {
|
||||
mtd_start(partition_names_default, n_partitions_default);
|
||||
}
|
||||
@@ -104,28 +120,49 @@ int mtd_main(int argc, char *argv[])
|
||||
if (!strcmp(argv[1], "test"))
|
||||
mtd_test();
|
||||
|
||||
if (!strcmp(argv[1], "erase")) {
|
||||
if (argc < 3) {
|
||||
errx(1, "usage: mtd erase <PARTITION_PATH..>");
|
||||
if (!strcmp(argv[1], "readtest")) {
|
||||
if (argc >= 3) {
|
||||
mtd_readtest(argv + 2, argc - 2);
|
||||
} else {
|
||||
mtd_readtest(partition_names_default, n_partitions_default);
|
||||
}
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "rwtest")) {
|
||||
if (argc >= 3) {
|
||||
mtd_rwtest(argv + 2, argc - 2);
|
||||
} else {
|
||||
mtd_rwtest(partition_names_default, n_partitions_default);
|
||||
}
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "status"))
|
||||
mtd_status();
|
||||
|
||||
if (!strcmp(argv[1], "erase")) {
|
||||
if (argc >= 3) {
|
||||
mtd_erase(argv + 2, argc - 2);
|
||||
} else {
|
||||
mtd_erase(partition_names_default, n_partitions_default);
|
||||
}
|
||||
mtd_erase(argv + 2, argc - 2);
|
||||
}
|
||||
}
|
||||
|
||||
errx(1, "expected a command, try 'start', 'erase' or 'test'");
|
||||
errx(1, "expected a command, try 'start', 'erase', 'status', 'readtest', 'rwtest' or 'test'");
|
||||
}
|
||||
|
||||
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
|
||||
struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd,
|
||||
off_t firstblock, off_t nblocks);
|
||||
|
||||
#ifdef CONFIG_MTD_RAMTRON
|
||||
static void
|
||||
mtd_attach(void)
|
||||
ramtron_attach(void)
|
||||
{
|
||||
/* find the right spi */
|
||||
struct spi_dev_s *spi = up_spiinitialize(2);
|
||||
/* this resets the spi bus, set correct bus speed again */
|
||||
SPI_SETFREQUENCY(spi, 40 * 1000 * 1000);
|
||||
SPI_SETFREQUENCY(spi, 10 * 1000 * 1000);
|
||||
SPI_SETBITS(spi, 8);
|
||||
SPI_SETMODE(spi, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi, SPIDEV_FLASH, false);
|
||||
@@ -133,7 +170,7 @@ mtd_attach(void)
|
||||
if (spi == NULL)
|
||||
errx(1, "failed to locate spi bus");
|
||||
|
||||
/* start the MTD driver, attempt 5 times */
|
||||
/* start the RAMTRON driver, attempt 5 times */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
mtd_dev = ramtron_initialize(spi);
|
||||
|
||||
@@ -151,8 +188,44 @@ mtd_attach(void)
|
||||
if (mtd_dev == NULL)
|
||||
errx(1, "failed to initialize mtd driver");
|
||||
|
||||
int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000);
|
||||
if (ret != OK)
|
||||
warnx(1, "failed to set bus speed");
|
||||
|
||||
attached = true;
|
||||
}
|
||||
#else
|
||||
|
||||
static void
|
||||
at24xxx_attach(void)
|
||||
{
|
||||
/* find the right I2C */
|
||||
struct i2c_dev_s *i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
|
||||
/* this resets the I2C bus, set correct bus speed again */
|
||||
I2C_SETFREQUENCY(i2c, 400000);
|
||||
|
||||
if (i2c == NULL)
|
||||
errx(1, "failed to locate I2C bus");
|
||||
|
||||
/* start the MTD driver, attempt 5 times */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
mtd_dev = at24c_initialize(i2c);
|
||||
if (mtd_dev) {
|
||||
/* abort on first valid result */
|
||||
if (i > 0) {
|
||||
warnx("warning: EEPROM needed %d attempts to attach", i+1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* if last attempt is still unsuccessful, abort */
|
||||
if (mtd_dev == NULL)
|
||||
errx(1, "failed to initialize EEPROM driver");
|
||||
|
||||
attached = true;
|
||||
}
|
||||
#endif
|
||||
|
||||
static void
|
||||
mtd_start(char *partition_names[], unsigned n_partitions)
|
||||
@@ -162,42 +235,25 @@ mtd_start(char *partition_names[], unsigned n_partitions)
|
||||
if (started)
|
||||
errx(1, "mtd already mounted");
|
||||
|
||||
if (!attached)
|
||||
mtd_attach();
|
||||
if (!attached) {
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
at24xxx_attach();
|
||||
#else
|
||||
ramtron_attach();
|
||||
#endif
|
||||
}
|
||||
|
||||
if (!mtd_dev) {
|
||||
warnx("ERROR: Failed to create RAMTRON FRAM MTD instance");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
unsigned long blocksize, erasesize, neraseblocks;
|
||||
unsigned blkpererase, nblocks, partsize;
|
||||
|
||||
/* Get the geometry of the FLASH device */
|
||||
|
||||
FAR struct mtd_geometry_s geo;
|
||||
|
||||
ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("ERROR: mtd->ioctl failed: %d", ret);
|
||||
ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions);
|
||||
if (ret)
|
||||
exit(3);
|
||||
}
|
||||
|
||||
warnx("Flash Geometry:");
|
||||
warnx(" blocksize: %lu", (unsigned long)geo.blocksize);
|
||||
warnx(" erasesize: %lu", (unsigned long)geo.erasesize);
|
||||
warnx(" neraseblocks: %lu", (unsigned long)geo.neraseblocks);
|
||||
|
||||
/* Determine the size of each partition. Make each partition an even
|
||||
* multiple of the erase block size (perhaps not using some space at the
|
||||
* end of the FLASH).
|
||||
*/
|
||||
|
||||
unsigned blkpererase = geo.erasesize / geo.blocksize;
|
||||
unsigned nblocks = (geo.neraseblocks / n_partitions) * blkpererase;
|
||||
unsigned partsize = nblocks * geo.blocksize;
|
||||
|
||||
warnx(" No. partitions: %u", n_partitions);
|
||||
warnx(" Partition size: %lu Blocks (%lu bytes)", (unsigned long)nblocks, (unsigned long)partsize);
|
||||
|
||||
/* Now create MTD FLASH partitions */
|
||||
|
||||
@@ -244,18 +300,98 @@ mtd_start(char *partition_names[], unsigned n_partitions)
|
||||
}
|
||||
}
|
||||
|
||||
n_partitions_current = n_partitions;
|
||||
|
||||
started = true;
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
int mtd_get_geometry(unsigned long *blocksize, unsigned long *erasesize, unsigned long *neraseblocks,
|
||||
unsigned *blkpererase, unsigned *nblocks, unsigned *partsize, unsigned n_partitions)
|
||||
{
|
||||
/* Get the geometry of the FLASH device */
|
||||
|
||||
FAR struct mtd_geometry_s geo;
|
||||
|
||||
int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo));
|
||||
|
||||
if (ret < 0) {
|
||||
warnx("ERROR: mtd->ioctl failed: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
*blocksize = geo.blocksize;
|
||||
*erasesize = geo.blocksize;
|
||||
*neraseblocks = geo.neraseblocks;
|
||||
|
||||
/* Determine the size of each partition. Make each partition an even
|
||||
* multiple of the erase block size (perhaps not using some space at the
|
||||
* end of the FLASH).
|
||||
*/
|
||||
|
||||
*blkpererase = geo.erasesize / geo.blocksize;
|
||||
*nblocks = (geo.neraseblocks / n_partitions) * *blkpererase;
|
||||
*partsize = *nblocks * geo.blocksize;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
get partition size in bytes
|
||||
*/
|
||||
static ssize_t mtd_get_partition_size(void)
|
||||
{
|
||||
unsigned long blocksize, erasesize, neraseblocks;
|
||||
unsigned blkpererase, nblocks, partsize = 0;
|
||||
|
||||
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
|
||||
if (ret != OK) {
|
||||
errx(1, "Failed to get geometry");
|
||||
}
|
||||
return partsize;
|
||||
}
|
||||
|
||||
void mtd_print_info()
|
||||
{
|
||||
if (!attached)
|
||||
exit(1);
|
||||
|
||||
unsigned long blocksize, erasesize, neraseblocks;
|
||||
unsigned blkpererase, nblocks, partsize;
|
||||
|
||||
int ret = mtd_get_geometry(&blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize, n_partitions_current);
|
||||
if (ret)
|
||||
exit(3);
|
||||
|
||||
warnx("Flash Geometry:");
|
||||
|
||||
printf(" blocksize: %lu\n", blocksize);
|
||||
printf(" erasesize: %lu\n", erasesize);
|
||||
printf(" neraseblocks: %lu\n", neraseblocks);
|
||||
printf(" No. partitions: %u\n", n_partitions_current);
|
||||
printf(" Partition size: %u Blocks (%u bytes)\n", nblocks, partsize);
|
||||
printf(" TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024);
|
||||
|
||||
}
|
||||
|
||||
void
|
||||
mtd_test(void)
|
||||
{
|
||||
warnx("This test routine does not test anything yet!");
|
||||
exit(1);
|
||||
}
|
||||
|
||||
static void
|
||||
void
|
||||
mtd_status(void)
|
||||
{
|
||||
if (!attached)
|
||||
errx(1, "MTD driver not started");
|
||||
|
||||
mtd_print_info();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
void
|
||||
mtd_erase(char *partition_names[], unsigned n_partitions)
|
||||
{
|
||||
uint8_t v[64];
|
||||
@@ -267,7 +403,7 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
|
||||
if (fd == -1) {
|
||||
errx(1, "Failed to open partition");
|
||||
}
|
||||
while (write(fd, &v, sizeof(v)) == sizeof(v)) {
|
||||
while (write(fd, v, sizeof(v)) == sizeof(v)) {
|
||||
count += sizeof(v);
|
||||
}
|
||||
printf("Erased %lu bytes\n", (unsigned long)count);
|
||||
@@ -276,4 +412,82 @@ mtd_erase(char *partition_names[], unsigned n_partitions)
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/*
|
||||
readtest is useful during startup to validate the device is
|
||||
responding on the bus. It relies on the driver returning an error on
|
||||
bad reads (the ramtron driver does return an error)
|
||||
*/
|
||||
void
|
||||
mtd_readtest(char *partition_names[], unsigned n_partitions)
|
||||
{
|
||||
ssize_t expected_size = mtd_get_partition_size();
|
||||
|
||||
uint8_t v[128];
|
||||
for (uint8_t i = 0; i < n_partitions; i++) {
|
||||
uint32_t count = 0;
|
||||
printf("reading %s expecting %u bytes\n", partition_names[i], expected_size);
|
||||
int fd = open(partition_names[i], O_RDONLY);
|
||||
if (fd == -1) {
|
||||
errx(1, "Failed to open partition");
|
||||
}
|
||||
while (read(fd, v, sizeof(v)) == sizeof(v)) {
|
||||
count += sizeof(v);
|
||||
}
|
||||
if (count != expected_size) {
|
||||
errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
|
||||
}
|
||||
close(fd);
|
||||
}
|
||||
printf("readtest OK\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/*
|
||||
rwtest is useful during startup to validate the device is
|
||||
responding on the bus for both reads and writes. It reads data in
|
||||
blocks and writes the data back, then reads it again, failing if the
|
||||
data isn't the same
|
||||
*/
|
||||
void
|
||||
mtd_rwtest(char *partition_names[], unsigned n_partitions)
|
||||
{
|
||||
ssize_t expected_size = mtd_get_partition_size();
|
||||
|
||||
uint8_t v[128], v2[128];
|
||||
for (uint8_t i = 0; i < n_partitions; i++) {
|
||||
uint32_t count = 0;
|
||||
off_t offset = 0;
|
||||
printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size);
|
||||
int fd = open(partition_names[i], O_RDWR);
|
||||
if (fd == -1) {
|
||||
errx(1, "Failed to open partition");
|
||||
}
|
||||
while (read(fd, v, sizeof(v)) == sizeof(v)) {
|
||||
count += sizeof(v);
|
||||
if (lseek(fd, offset, SEEK_SET) != offset) {
|
||||
errx(1, "seek failed");
|
||||
}
|
||||
if (write(fd, v, sizeof(v)) != sizeof(v)) {
|
||||
errx(1, "write failed");
|
||||
}
|
||||
if (lseek(fd, offset, SEEK_SET) != offset) {
|
||||
errx(1, "seek failed");
|
||||
}
|
||||
if (read(fd, v2, sizeof(v2)) != sizeof(v2)) {
|
||||
errx(1, "read failed");
|
||||
}
|
||||
if (memcmp(v, v2, sizeof(v2)) != 0) {
|
||||
errx(1, "memcmp failed");
|
||||
}
|
||||
offset += sizeof(v);
|
||||
}
|
||||
if (count != expected_size) {
|
||||
errx(1,"Failed to read partition - got %u/%u bytes", count, expected_size);
|
||||
}
|
||||
close(fd);
|
||||
}
|
||||
printf("rwtest OK\n");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@@ -1,6 +0,0 @@
|
||||
#
|
||||
# RAMTRON file system driver
|
||||
#
|
||||
|
||||
MODULE_COMMAND = ramtron
|
||||
SRCS = ramtron.c
|
||||
@@ -1,279 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Lorenz Meier <lm@inf.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. 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.
|
||||
* 3. Neither the name PX4 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 COPYRIGHT HOLDERS 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
|
||||
* COPYRIGHT OWNER OR 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ramtron.c
|
||||
*
|
||||
* ramtron service and utility app.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <sys/mount.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <sys/stat.h>
|
||||
|
||||
#include <nuttx/spi.h>
|
||||
#include <nuttx/mtd.h>
|
||||
#include <nuttx/fs/nxffs.h>
|
||||
#include <nuttx/fs/ioctl.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "systemlib/systemlib.h"
|
||||
#include "systemlib/param/param.h"
|
||||
#include "systemlib/err.h"
|
||||
|
||||
__EXPORT int ramtron_main(int argc, char *argv[]);
|
||||
|
||||
#ifndef CONFIG_MTD_RAMTRON
|
||||
|
||||
/* create a fake command with decent message to not confuse users */
|
||||
int ramtron_main(int argc, char *argv[])
|
||||
{
|
||||
errx(1, "RAMTRON not enabled, skipping.");
|
||||
}
|
||||
#else
|
||||
|
||||
static void ramtron_attach(void);
|
||||
static void ramtron_start(void);
|
||||
static void ramtron_erase(void);
|
||||
static void ramtron_ioctl(unsigned operation);
|
||||
static void ramtron_save(const char *name);
|
||||
static void ramtron_load(const char *name);
|
||||
static void ramtron_test(void);
|
||||
|
||||
static bool attached = false;
|
||||
static bool started = false;
|
||||
static struct mtd_dev_s *ramtron_mtd;
|
||||
|
||||
int ramtron_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc >= 2) {
|
||||
if (!strcmp(argv[1], "start"))
|
||||
ramtron_start();
|
||||
|
||||
if (!strcmp(argv[1], "save_param"))
|
||||
ramtron_save(argv[2]);
|
||||
|
||||
if (!strcmp(argv[1], "load_param"))
|
||||
ramtron_load(argv[2]);
|
||||
|
||||
if (!strcmp(argv[1], "erase"))
|
||||
ramtron_erase();
|
||||
|
||||
if (!strcmp(argv[1], "test"))
|
||||
ramtron_test();
|
||||
|
||||
if (0) { /* these actually require a file on the filesystem... */
|
||||
|
||||
if (!strcmp(argv[1], "reformat"))
|
||||
ramtron_ioctl(FIOC_REFORMAT);
|
||||
|
||||
if (!strcmp(argv[1], "repack"))
|
||||
ramtron_ioctl(FIOC_OPTIMIZE);
|
||||
}
|
||||
}
|
||||
|
||||
errx(1, "expected a command, try 'start'\n\t'save_param /ramtron/parameters'\n\t'load_param /ramtron/parameters'\n\t'erase'\n");
|
||||
}
|
||||
|
||||
struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev);
|
||||
|
||||
|
||||
static void
|
||||
ramtron_attach(void)
|
||||
{
|
||||
/* find the right spi */
|
||||
struct spi_dev_s *spi = up_spiinitialize(2);
|
||||
/* this resets the spi bus, set correct bus speed again */
|
||||
// xxx set in ramtron driver, leave this out
|
||||
// SPI_SETFREQUENCY(spi, 4000000);
|
||||
SPI_SETFREQUENCY(spi, 375000000);
|
||||
SPI_SETBITS(spi, 8);
|
||||
SPI_SETMODE(spi, SPIDEV_MODE3);
|
||||
SPI_SELECT(spi, SPIDEV_FLASH, false);
|
||||
|
||||
if (spi == NULL)
|
||||
errx(1, "failed to locate spi bus");
|
||||
|
||||
/* start the MTD driver, attempt 5 times */
|
||||
for (int i = 0; i < 5; i++) {
|
||||
ramtron_mtd = ramtron_initialize(spi);
|
||||
if (ramtron_mtd) {
|
||||
/* abort on first valid result */
|
||||
if (i > 0) {
|
||||
warnx("warning: ramtron needed %d attempts to attach", i+1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/* if last attempt is still unsuccessful, abort */
|
||||
if (ramtron_mtd == NULL)
|
||||
errx(1, "failed to initialize ramtron driver");
|
||||
|
||||
attached = true;
|
||||
}
|
||||
|
||||
static void
|
||||
ramtron_start(void)
|
||||
{
|
||||
int ret;
|
||||
|
||||
if (started)
|
||||
errx(1, "ramtron already mounted");
|
||||
|
||||
if (!attached)
|
||||
ramtron_attach();
|
||||
|
||||
/* start NXFFS */
|
||||
ret = nxffs_initialize(ramtron_mtd);
|
||||
|
||||
if (ret < 0)
|
||||
errx(1, "failed to initialize NXFFS - erase ramtron to reformat");
|
||||
|
||||
/* mount the ramtron */
|
||||
ret = mount(NULL, "/ramtron", "nxffs", 0, NULL);
|
||||
|
||||
if (ret < 0)
|
||||
errx(1, "failed to mount /ramtron - erase ramtron to reformat");
|
||||
|
||||
started = true;
|
||||
warnx("mounted ramtron at /ramtron");
|
||||
exit(0);
|
||||
}
|
||||
|
||||
//extern int at24c_nuke(void);
|
||||
|
||||
static void
|
||||
ramtron_erase(void)
|
||||
{
|
||||
if (!attached)
|
||||
ramtron_attach();
|
||||
|
||||
// if (at24c_nuke())
|
||||
errx(1, "erase failed");
|
||||
|
||||
errx(0, "erase done, reboot now");
|
||||
}
|
||||
|
||||
static void
|
||||
ramtron_ioctl(unsigned operation)
|
||||
{
|
||||
int fd;
|
||||
|
||||
fd = open("/ramtron/.", 0);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "open /ramtron");
|
||||
|
||||
if (ioctl(fd, operation, 0) < 0)
|
||||
err(1, "ioctl");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
ramtron_save(const char *name)
|
||||
{
|
||||
if (!started)
|
||||
errx(1, "must be started first");
|
||||
|
||||
if (!name)
|
||||
err(1, "missing argument for device name, try '/ramtron/parameters'");
|
||||
|
||||
warnx("WARNING: 'ramtron save_param' deprecated - use 'param save' instead");
|
||||
|
||||
/* delete the file in case it exists */
|
||||
unlink(name);
|
||||
|
||||
/* create the file */
|
||||
int fd = open(name, O_WRONLY | O_CREAT | O_EXCL);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "opening '%s' failed", name);
|
||||
|
||||
int result = param_export(fd, false);
|
||||
close(fd);
|
||||
|
||||
if (result < 0) {
|
||||
unlink(name);
|
||||
errx(1, "error exporting to '%s'", name);
|
||||
}
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
static void
|
||||
ramtron_load(const char *name)
|
||||
{
|
||||
if (!started)
|
||||
errx(1, "must be started first");
|
||||
|
||||
if (!name)
|
||||
err(1, "missing argument for device name, try '/ramtron/parameters'");
|
||||
|
||||
warnx("WARNING: 'ramtron load_param' deprecated - use 'param load' instead");
|
||||
|
||||
int fd = open(name, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "open '%s'", name);
|
||||
|
||||
int result = param_load(fd);
|
||||
close(fd);
|
||||
|
||||
if (result < 0)
|
||||
errx(1, "error importing from '%s'", name);
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
//extern void at24c_test(void);
|
||||
|
||||
static void
|
||||
ramtron_test(void)
|
||||
{
|
||||
// at24c_test();
|
||||
exit(0);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -30,4 +30,6 @@ SRCS = test_adc.c \
|
||||
test_param.c \
|
||||
test_ppm_loopback.c \
|
||||
test_rc.c \
|
||||
test_mount.c
|
||||
test_conv.cpp \
|
||||
test_mount.c \
|
||||
test_mtd.c
|
||||
|
||||
@@ -0,0 +1,76 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. 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.
|
||||
* 3. Neither the name PX4 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 COPYRIGHT HOLDERS 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
|
||||
* COPYRIGHT OWNER OR 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file test_conv.cpp
|
||||
* Tests conversions used across the system.
|
||||
*
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include "tests.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <float.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <unit_test/unit_test.h>
|
||||
#include <px4iofirmware/protocol.h>
|
||||
|
||||
int test_conv(int argc, char *argv[])
|
||||
{
|
||||
warnx("Testing system conversions");
|
||||
|
||||
for (int i = -10000; i <= 10000; i+=1) {
|
||||
float f = i/10000.0f;
|
||||
float fres = REG_TO_FLOAT(FLOAT_TO_REG(f));
|
||||
if (fabsf(f - fres) > 0.0001f) {
|
||||
warnx("conversion fail: input: %8.4f, intermediate: %d, result: %8.4f", f, REG_TO_SIGNED(FLOAT_TO_REG(f)), fres);
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
warnx("All conversions clean");
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -54,7 +54,7 @@
|
||||
|
||||
#include "tests.h"
|
||||
|
||||
int check_user_abort(int fd);
|
||||
static int check_user_abort(int fd);
|
||||
|
||||
int check_user_abort(int fd) {
|
||||
/* check if user wants to abort */
|
||||
|
||||
@@ -48,9 +48,13 @@
|
||||
#include <errno.h>
|
||||
#include <string.h>
|
||||
#include <time.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/mixer/mixer.h>
|
||||
#include <systemlib/pwm_limit/pwm_limit.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "tests.h"
|
||||
|
||||
@@ -59,6 +63,20 @@ static int mixer_callback(uintptr_t handle,
|
||||
uint8_t control_index,
|
||||
float &control);
|
||||
|
||||
const unsigned output_max = 8;
|
||||
static float actuator_controls[output_max];
|
||||
static bool should_arm = false;
|
||||
uint16_t r_page_servo_disarmed[output_max];
|
||||
uint16_t r_page_servo_control_min[output_max];
|
||||
uint16_t r_page_servo_control_max[output_max];
|
||||
uint16_t r_page_servos[output_max];
|
||||
uint16_t servo_predicted[output_max];
|
||||
|
||||
/*
|
||||
* PWM limit structure
|
||||
*/
|
||||
pwm_limit_t pwm_limit;
|
||||
|
||||
int test_mixer(int argc, char *argv[])
|
||||
{
|
||||
warnx("testing mixer");
|
||||
@@ -164,6 +182,174 @@ int test_mixer(int argc, char *argv[])
|
||||
if (mixer_group.count() != 8)
|
||||
return 1;
|
||||
|
||||
/* execute the mixer */
|
||||
|
||||
float outputs[output_max];
|
||||
unsigned mixed;
|
||||
const int jmax = 5;
|
||||
|
||||
pwm_limit_init(&pwm_limit);
|
||||
should_arm = true;
|
||||
|
||||
/* run through arming phase */
|
||||
for (int i = 0; i < output_max; i++) {
|
||||
actuator_controls[i] = 0.1f;
|
||||
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
|
||||
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
|
||||
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
|
||||
}
|
||||
|
||||
warnx("ARMING TEST: STARTING RAMP");
|
||||
unsigned sleep_quantum_us = 10000;
|
||||
|
||||
hrt_abstime starttime = hrt_absolute_time();
|
||||
unsigned sleepcount = 0;
|
||||
|
||||
while (hrt_elapsed_time(&starttime) < INIT_TIME_US + RAMP_TIME_US) {
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
|
||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||
for (int i = 0; i < mixed; i++)
|
||||
{
|
||||
/* check mixed outputs to be zero during init phase */
|
||||
if (hrt_elapsed_time(&starttime) < INIT_TIME_US &&
|
||||
r_page_servos[i] != r_page_servo_disarmed[i]) {
|
||||
warnx("disarmed servo value mismatch");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (hrt_elapsed_time(&starttime) >= INIT_TIME_US &&
|
||||
r_page_servos[i] + 1 <= r_page_servo_disarmed[i]) {
|
||||
warnx("ramp servo value mismatch");
|
||||
return 1;
|
||||
}
|
||||
|
||||
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
|
||||
}
|
||||
usleep(sleep_quantum_us);
|
||||
sleepcount++;
|
||||
|
||||
if (sleepcount % 10 == 0) {
|
||||
printf(".");
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
warnx("ARMING TEST: NORMAL OPERATION");
|
||||
|
||||
for (int j = -jmax; j <= jmax; j++) {
|
||||
|
||||
for (int i = 0; i < output_max; i++) {
|
||||
actuator_controls[i] = j/10.0f + 0.1f * i;
|
||||
r_page_servo_disarmed[i] = PWM_LOWEST_MIN;
|
||||
r_page_servo_control_min[i] = PWM_DEFAULT_MIN;
|
||||
r_page_servo_control_max[i] = PWM_DEFAULT_MAX;
|
||||
}
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
|
||||
warnx("mixed %d outputs (max %d)", mixed, output_max);
|
||||
for (int i = 0; i < mixed; i++)
|
||||
{
|
||||
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
|
||||
if (fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
|
||||
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
|
||||
warnx("mixer violated predicted value");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
warnx("ARMING TEST: DISARMING");
|
||||
|
||||
starttime = hrt_absolute_time();
|
||||
sleepcount = 0;
|
||||
should_arm = false;
|
||||
|
||||
while (hrt_elapsed_time(&starttime) < 600000) {
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
|
||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||
for (int i = 0; i < mixed; i++)
|
||||
{
|
||||
/* check mixed outputs to be zero during init phase */
|
||||
if (r_page_servos[i] != r_page_servo_disarmed[i]) {
|
||||
warnx("disarmed servo value mismatch");
|
||||
return 1;
|
||||
}
|
||||
|
||||
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
|
||||
}
|
||||
usleep(sleep_quantum_us);
|
||||
sleepcount++;
|
||||
|
||||
if (sleepcount % 10 == 0) {
|
||||
printf(".");
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
warnx("ARMING TEST: REARMING: STARTING RAMP");
|
||||
|
||||
starttime = hrt_absolute_time();
|
||||
sleepcount = 0;
|
||||
should_arm = true;
|
||||
|
||||
while (hrt_elapsed_time(&starttime) < 600000 + RAMP_TIME_US) {
|
||||
|
||||
/* mix */
|
||||
mixed = mixer_group.mix(&outputs[0], output_max);
|
||||
|
||||
pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
|
||||
|
||||
//warnx("mixed %d outputs (max %d), values:", mixed, output_max);
|
||||
for (int i = 0; i < mixed; i++)
|
||||
{
|
||||
/* predict value */
|
||||
servo_predicted[i] = 1500 + outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i]) / 2.0f;
|
||||
|
||||
/* check ramp */
|
||||
|
||||
if (hrt_elapsed_time(&starttime) < RAMP_TIME_US &&
|
||||
(r_page_servos[i] + 1 <= r_page_servo_disarmed[i] ||
|
||||
r_page_servos[i] > servo_predicted[i])) {
|
||||
warnx("ramp servo value mismatch");
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* check post ramp phase */
|
||||
if (hrt_elapsed_time(&starttime) > RAMP_TIME_US &&
|
||||
fabsf(servo_predicted[i] - r_page_servos[i]) > 2) {
|
||||
printf("\t %d: %8.4f predicted: %d, servo: %d\n", i, outputs[i], servo_predicted[i], (int)r_page_servos[i]);
|
||||
warnx("mixer violated predicted value");
|
||||
return 1;
|
||||
}
|
||||
|
||||
//printf("\t %d: %8.4f limited: %8.4f, servo: %d\n", i, outputs_unlimited[i], outputs[i], (int)r_page_servos[i]);
|
||||
}
|
||||
usleep(sleep_quantum_us);
|
||||
sleepcount++;
|
||||
|
||||
if (sleepcount % 10 == 0) {
|
||||
printf(".");
|
||||
fflush(stdout);
|
||||
}
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
/* load multirotor at once test */
|
||||
mixer_group.reset();
|
||||
|
||||
@@ -180,8 +366,12 @@ int test_mixer(int argc, char *argv[])
|
||||
unsigned mc_loaded = loaded;
|
||||
mixer_group.load_from_buf(&buf[0], mc_loaded);
|
||||
warnx("complete buffer load: loaded %u mixers", mixer_group.count());
|
||||
if (mixer_group.count() != 8)
|
||||
if (mixer_group.count() != 5) {
|
||||
warnx("FAIL: Quad W mixer load failed");
|
||||
return 1;
|
||||
}
|
||||
|
||||
warnx("SUCCESS: No errors in mixer test");
|
||||
}
|
||||
|
||||
static int
|
||||
@@ -193,7 +383,10 @@ mixer_callback(uintptr_t handle,
|
||||
if (control_group != 0)
|
||||
return -1;
|
||||
|
||||
control = 0.0f;
|
||||
if (control_index > (sizeof(actuator_controls) / sizeof(actuator_controls[0])))
|
||||
return -1;
|
||||
|
||||
control = actuator_controls[control_index];
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,229 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. 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.
|
||||
* 3. Neither the name PX4 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 COPYRIGHT HOLDERS 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
|
||||
* COPYRIGHT OWNER OR 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file test_mtd.c
|
||||
*
|
||||
* Param storage / file test.
|
||||
*
|
||||
* @author Lorenz Meier <lm@inf.ethz.ch>
|
||||
*/
|
||||
|
||||
#include <sys/stat.h>
|
||||
#include <poll.h>
|
||||
#include <dirent.h>
|
||||
#include <stdio.h>
|
||||
#include <stddef.h>
|
||||
#include <unistd.h>
|
||||
#include <fcntl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include "tests.h"
|
||||
|
||||
#define PARAM_FILE_NAME "/fs/mtd_params"
|
||||
|
||||
static int check_user_abort(int fd);
|
||||
|
||||
int check_user_abort(int fd) {
|
||||
/* check if user wants to abort */
|
||||
char c;
|
||||
|
||||
struct pollfd fds;
|
||||
int ret;
|
||||
fds.fd = 0; /* stdin */
|
||||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 0);
|
||||
|
||||
if (ret > 0) {
|
||||
|
||||
read(0, &c, 1);
|
||||
|
||||
switch (c) {
|
||||
case 0x03: // ctrl-c
|
||||
case 0x1b: // esc
|
||||
case 'c':
|
||||
case 'q':
|
||||
{
|
||||
warnx("Test aborted.");
|
||||
fsync(fd);
|
||||
close(fd);
|
||||
return OK;
|
||||
/* not reached */
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
void print_fail()
|
||||
{
|
||||
printf("<[T]: MTD: FAIL>\n");
|
||||
}
|
||||
|
||||
void print_success()
|
||||
{
|
||||
printf("<[T]: MTD: OK>\n");
|
||||
}
|
||||
|
||||
int
|
||||
test_mtd(int argc, char *argv[])
|
||||
{
|
||||
unsigned iterations= 0;
|
||||
|
||||
/* check if microSD card is mounted */
|
||||
struct stat buffer;
|
||||
if (stat(PARAM_FILE_NAME, &buffer)) {
|
||||
warnx("file %s not found, aborting MTD test", PARAM_FILE_NAME);
|
||||
print_fail();
|
||||
return 1;
|
||||
}
|
||||
|
||||
// XXX get real storage space here
|
||||
unsigned file_size = 4096;
|
||||
|
||||
/* perform tests for a range of chunk sizes */
|
||||
unsigned chunk_sizes[] = {256, 512, 4096};
|
||||
|
||||
for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) {
|
||||
|
||||
printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]);
|
||||
|
||||
uint8_t write_buf[chunk_sizes[c]] __attribute__((aligned(64)));
|
||||
|
||||
/* fill write buffer with known values */
|
||||
for (int i = 0; i < sizeof(write_buf); i++) {
|
||||
/* this will wrap, but we just need a known value with spacing */
|
||||
write_buf[i] = i+11;
|
||||
}
|
||||
|
||||
uint8_t read_buf[chunk_sizes[c]] __attribute__((aligned(64)));
|
||||
hrt_abstime start, end;
|
||||
|
||||
int fd = open(PARAM_FILE_NAME, O_RDONLY);
|
||||
int rret = read(fd, read_buf, chunk_sizes[c]);
|
||||
close(fd);
|
||||
|
||||
fd = open(PARAM_FILE_NAME, O_WRONLY);
|
||||
|
||||
printf("printing 2 percent of the first chunk:\n");
|
||||
for (int i = 0; i < sizeof(read_buf) / 50; i++) {
|
||||
printf("%02X", read_buf[i]);
|
||||
}
|
||||
printf("\n");
|
||||
|
||||
iterations = file_size / chunk_sizes[c];
|
||||
|
||||
start = hrt_absolute_time();
|
||||
for (unsigned i = 0; i < iterations; i++) {
|
||||
int wret = write(fd, write_buf, chunk_sizes[c]);
|
||||
|
||||
if (wret != (int)chunk_sizes[c]) {
|
||||
warn("WRITE ERROR!");
|
||||
print_fail();
|
||||
return 1;
|
||||
}
|
||||
|
||||
fsync(fd);
|
||||
|
||||
if (!check_user_abort(fd))
|
||||
return OK;
|
||||
|
||||
}
|
||||
end = hrt_absolute_time();
|
||||
|
||||
close(fd);
|
||||
fd = open(PARAM_FILE_NAME, O_RDONLY);
|
||||
|
||||
/* read back data for validation */
|
||||
for (unsigned i = 0; i < iterations; i++) {
|
||||
int rret = read(fd, read_buf, chunk_sizes[c]);
|
||||
|
||||
if (rret != chunk_sizes[c]) {
|
||||
warnx("READ ERROR!");
|
||||
print_fail();
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* compare value */
|
||||
bool compare_ok = true;
|
||||
|
||||
for (int j = 0; j < chunk_sizes[c]; j++) {
|
||||
if (read_buf[j] != write_buf[j]) {
|
||||
warnx("COMPARISON ERROR: byte %d", j);
|
||||
print_fail();
|
||||
compare_ok = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!compare_ok) {
|
||||
warnx("ABORTING FURTHER COMPARISON DUE TO ERROR");
|
||||
print_fail();
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!check_user_abort(fd))
|
||||
return OK;
|
||||
|
||||
}
|
||||
|
||||
|
||||
close(fd);
|
||||
|
||||
}
|
||||
|
||||
/* fill the file with 0xFF to make it look new again */
|
||||
char ffbuf[64];
|
||||
memset(ffbuf, 0xFF, sizeof(ffbuf));
|
||||
int fd = open(PARAM_FILE_NAME, O_WRONLY);
|
||||
for (int i = 0; i < file_size / sizeof(ffbuf); i++) {
|
||||
int ret = write(fd, ffbuf, sizeof(ffbuf));
|
||||
|
||||
if (ret != sizeof(ffbuf)) {
|
||||
warnx("ERROR! Could not fill file with 0xFF");
|
||||
close(fd);
|
||||
print_fail();
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
(void)close(fd);
|
||||
print_success();
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -32,8 +32,8 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file test_ppm_loopback.c
|
||||
* Tests the PWM outputs and PPM input
|
||||
* @file test_rc.c
|
||||
* Tests RC input.
|
||||
*
|
||||
*/
|
||||
|
||||
|
||||
@@ -109,7 +109,9 @@ extern int test_bson(int argc, char *argv[]);
|
||||
extern int test_file(int argc, char *argv[]);
|
||||
extern int test_mixer(int argc, char *argv[]);
|
||||
extern int test_rc(int argc, char *argv[]);
|
||||
extern int test_conv(int argc, char *argv[]);
|
||||
extern int test_mount(int argc, char *argv[]);
|
||||
extern int test_mtd(int argc, char *argv[]);
|
||||
extern int test_mathlib(int argc, char *argv[]);
|
||||
|
||||
__END_DECLS
|
||||
|
||||
@@ -106,7 +106,9 @@ const struct {
|
||||
{"file", test_file, 0},
|
||||
{"mixer", test_mixer, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"rc", test_rc, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"conv", test_conv, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"mount", test_mount, OPT_NOJIGTEST | OPT_NOALLTEST},
|
||||
{"mtd", test_mtd, 0},
|
||||
{"mathlib", test_mathlib, 0},
|
||||
{"help", test_help, OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
|
||||
{NULL, NULL, 0}
|
||||
|
||||
Reference in New Issue
Block a user