forked from Archive/PX4-Autopilot
Merge branch 'px4io-i2c' of github.com:PX4/Firmware into px4io-i2c
This commit is contained in:
Lorenz Meier
commit
c9775d7455
204
Debug/ARMv7M
204
Debug/ARMv7M
|
|
@ -19,120 +19,132 @@ end
|
|||
|
||||
|
||||
define vecstate
|
||||
set $icsr = *(uint32_t *)0xe000ed04
|
||||
set $icsr = *(unsigned *)0xe000ed04
|
||||
set $vect = $icsr & 0x1ff
|
||||
set $pend = ($icsr & 0x1ff000) >> 12
|
||||
set $shcsr = *(uint32_t *)0xe000ed24
|
||||
set $cfsr = *(uint32_t *)0xe000ed28
|
||||
set $shcsr = *(unsigned *)0xe000ed24
|
||||
set $cfsr = *(unsigned *)0xe000ed28
|
||||
set $mmfsr = $cfsr & 0xff
|
||||
set $bfsr = ($cfsr >> 8) & 0xff
|
||||
set $ufsr = ($cfsr >> 16) & 0xffff
|
||||
set $hfsr = *(uint32_t *)0xe000ed2c
|
||||
set $bfar = *(uint32_t *)0xe000ed38
|
||||
set $mmfar = *(uint32_t *)0xe000ed34
|
||||
set $hfsr = *(unsigned *)0xe000ed2c
|
||||
set $bfar = *(unsigned *)0xe000ed38
|
||||
set $mmfar = *(unsigned *)0xe000ed34
|
||||
|
||||
# XXX Currently, rather than look at $vect, we just decode the
|
||||
# fault status registers directly.
|
||||
if $vect < 15
|
||||
|
||||
if $hfsr != 0
|
||||
printf "HardFault:"
|
||||
if $hfsr & (1<<1)
|
||||
printf " due to vector table read fault\n"
|
||||
if $hfsr != 0
|
||||
printf "HardFault:"
|
||||
if $hfsr & (1<<1)
|
||||
printf " due to vector table read fault\n"
|
||||
end
|
||||
if $hfsr & (1<<30)
|
||||
printf " forced due to escalated or disabled configurable fault (see below)\n"
|
||||
end
|
||||
if $hfsr & (1<<31)
|
||||
printf " due to an unexpected debug event\n"
|
||||
end
|
||||
end
|
||||
if $hfsr & (1<<30)
|
||||
printf " forced ue to escalated or disabled configurable fault (see below)\n"
|
||||
if $mmfsr != 0
|
||||
printf "MemManage:"
|
||||
if $mmfsr & (1<<5)
|
||||
printf " during lazy FP state save"
|
||||
end
|
||||
if $mmfsr & (1<<4)
|
||||
printf " during exception entry"
|
||||
end
|
||||
if $mmfsr & (1<<3)
|
||||
printf " during exception return"
|
||||
end
|
||||
if $mmfsr & (1<<0)
|
||||
printf " during data access"
|
||||
end
|
||||
if $mmfsr & (1<<0)
|
||||
printf " during instruction prefetch"
|
||||
end
|
||||
if $mmfsr & (1<<7)
|
||||
printf " accessing 0x%08x", $mmfar
|
||||
end
|
||||
printf "\n"
|
||||
end
|
||||
if $hfsr & (1<<31)
|
||||
printf " due to an unexpected debug event\n"
|
||||
if $bfsr != 0
|
||||
printf "BusFault:"
|
||||
if $bfsr & (1<<2)
|
||||
printf " (imprecise)"
|
||||
end
|
||||
if $bfsr & (1<<1)
|
||||
printf " (precise)"
|
||||
end
|
||||
if $bfsr & (1<<5)
|
||||
printf " during lazy FP state save"
|
||||
end
|
||||
if $bfsr & (1<<4)
|
||||
printf " during exception entry"
|
||||
end
|
||||
if $bfsr & (1<<3)
|
||||
printf " during exception return"
|
||||
end
|
||||
if $bfsr & (1<<0)
|
||||
printf " during instruction prefetch"
|
||||
end
|
||||
if $bfsr & (1<<7)
|
||||
printf " accessing 0x%08x", $bfar
|
||||
end
|
||||
printf "\n"
|
||||
end
|
||||
if $ufsr != 0
|
||||
printf "UsageFault"
|
||||
if $ufsr & (1<<9)
|
||||
printf " due to divide-by-zero"
|
||||
end
|
||||
if $ufsr & (1<<8)
|
||||
printf " due to unaligned memory access"
|
||||
end
|
||||
if $ufsr & (1<<3)
|
||||
printf " due to access to disabled/absent coprocessor"
|
||||
end
|
||||
if $ufsr & (1<<2)
|
||||
printf " due to a bad EXC_RETURN value"
|
||||
end
|
||||
if $ufsr & (1<<1)
|
||||
printf " due to bad T or IT bits in EPSR"
|
||||
end
|
||||
if $ufsr & (1<<0)
|
||||
printf " due to executing an undefined instruction"
|
||||
end
|
||||
printf "\n"
|
||||
end
|
||||
else
|
||||
if $vect >= 15
|
||||
printf "Handling vector %u\n", $vect
|
||||
end
|
||||
end
|
||||
if $mmfsr != 0
|
||||
printf "MemManage:"
|
||||
if $mmfsr & (1<<5)
|
||||
printf " during lazy FP state save"
|
||||
end
|
||||
if $mmfsr & (1<<4)
|
||||
printf " during exception entry"
|
||||
end
|
||||
if $mmfsr & (1<<3)
|
||||
printf " during exception return"
|
||||
end
|
||||
if $mmfsr & (1<<0)
|
||||
printf " during data access"
|
||||
end
|
||||
if $mmfsr & (1<<0)
|
||||
printf " during instruction prefetch"
|
||||
end
|
||||
if $mmfsr & (1<<7)
|
||||
printf " accessing 0x%08x", $mmfar
|
||||
end
|
||||
printf "\n"
|
||||
end
|
||||
if $bfsr != 0
|
||||
printf "BusFault:"
|
||||
if $bfsr & (1<<2)
|
||||
printf " (imprecise)"
|
||||
end
|
||||
if $bfsr & (1<<1)
|
||||
printf " (precise)"
|
||||
end
|
||||
if $bfsr & (1<<5)
|
||||
printf " during lazy FP state save"
|
||||
end
|
||||
if $bfsr & (1<<4)
|
||||
printf " during exception entry"
|
||||
end
|
||||
if $bfsr & (1<<3)
|
||||
printf " during exception return"
|
||||
end
|
||||
if $bfsr & (1<<0)
|
||||
printf " during instruction prefetch"
|
||||
end
|
||||
if $bfsr & (1<<7)
|
||||
printf " accessing 0x%08x", $bfar
|
||||
end
|
||||
printf "\n"
|
||||
end
|
||||
if $ufsr != 0
|
||||
printf "UsageFault"
|
||||
if $ufsr & (1<<9)
|
||||
printf " due to divide-by-zero"
|
||||
end
|
||||
if $ufsr & (1<<8)
|
||||
printf " due to unaligned memory access"
|
||||
end
|
||||
if $ufsr & (1<<3)
|
||||
printf " due to access to disabled/absent coprocessor"
|
||||
end
|
||||
if $ufsr & (1<<2)
|
||||
printf " due to a bad EXC_RETURN value"
|
||||
end
|
||||
if $ufsr & (1<<1)
|
||||
printf " due to bad T or IT bits in EPSR"
|
||||
end
|
||||
if $ufsr & (1<<0)
|
||||
printf " due to executing an undefined instruction"
|
||||
end
|
||||
printf "\n"
|
||||
end
|
||||
if ((uint32_t)$lr & 0xf0000000) == 0xf0000000
|
||||
if ((unsigned)$lr & 0xf0000000) == 0xf0000000
|
||||
if ($lr & 1)
|
||||
set $frame_ptr = (uint32_t *)$msp
|
||||
printf "exception frame is on MSP\n"
|
||||
#set $frame_ptr = (unsigned *)$msp
|
||||
set $frame_ptr = (unsigned *)$sp
|
||||
else
|
||||
set $frame_ptr = (uint32_t *)$psp
|
||||
printf "exception frame is on PSP, backtrace may not be possible\n"
|
||||
#set $frame_ptr = (unsigned *)$psp
|
||||
set $frame_ptr = (unsigned *)$sp
|
||||
end
|
||||
printf " r0: %08x r1: %08x r2: %08x r3: %08x\n, $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3]
|
||||
if $lr & 0x10
|
||||
set $fault_sp = $frame_ptr + (8 * 4)
|
||||
else
|
||||
set $fault_sp = $frame_ptr + (26 * 4)
|
||||
end
|
||||
|
||||
|
||||
printf " r0: %08x r1: %08x r2: %08x r3: %08x\n", $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3]
|
||||
printf " r4: %08x r5: %08x r6: %08x r7: %08x\n", $r4, $r5, $r6, $r7
|
||||
printf " r8: %08x r9: %08x r10: %08x r11: %08x\n", $r8, $r9, $r10, $r11
|
||||
printf " r12: $08x lr: %08x pc: %08xx PSR: %08x\n", $frame_ptr[4], $frame_ptr[5], $frame_ptr[6], $frame_ptr[7]
|
||||
printf " r12: %08x\n", $frame_ptr[4]
|
||||
printf " sp: %08x lr: %08x pc: %08x PSR: %08x\n", $fault_sp, $frame_ptr[5], $frame_ptr[6], $frame_ptr[7]
|
||||
|
||||
# Swap to the context of the faulting code and try to print a backtrace
|
||||
set $saved_sp = $sp
|
||||
if $lr & 0x10
|
||||
set $sp = $frame_ptr + (8 * 4)
|
||||
else
|
||||
set $sp = $frame_ptr + (26 * 4)
|
||||
end
|
||||
set $sp = $fault_sp
|
||||
set $saved_lr = $lr
|
||||
set $lr = $frame_ptr[5]
|
||||
set $saved_pc = $pc
|
||||
|
|
@ -142,7 +154,7 @@ define vecstate
|
|||
set $lr = $saved_lr
|
||||
set $pc = $saved_pc
|
||||
else
|
||||
printf "(not currently in exception state)\n"
|
||||
printf "(not currently in exception handler)\n"
|
||||
end
|
||||
end
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,81 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 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 Rangefinder driver interface.
|
||||
*/
|
||||
|
||||
#ifndef _DRV_RANGEFINDER_H
|
||||
#define _DRV_RANGEFINDER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sys/ioctl.h>
|
||||
|
||||
#include "drv_sensor.h"
|
||||
#include "drv_orb_dev.h"
|
||||
|
||||
#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder"
|
||||
|
||||
/**
|
||||
* range finder report structure. Reads from the device must be in multiples of this
|
||||
* structure.
|
||||
*/
|
||||
struct range_finder_report {
|
||||
uint64_t timestamp;
|
||||
float distance; /** in meters */
|
||||
uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */
|
||||
};
|
||||
|
||||
/*
|
||||
* ObjDev tag for raw range finder data.
|
||||
*/
|
||||
ORB_DECLARE(sensor_range_finder);
|
||||
|
||||
/*
|
||||
* ioctl() definitions
|
||||
*
|
||||
* Rangefinder drivers also implement the generic sensor driver
|
||||
* interfaces from drv_sensor.h
|
||||
*/
|
||||
|
||||
#define _RANGEFINDERIOCBASE (0x7900)
|
||||
#define __RANGEFINDERIOC(_n) (_IOC(_RANGEFINDERIOCBASE, _n))
|
||||
|
||||
/** set the minimum effective distance of the device */
|
||||
#define RANGEFINDERIOCSETMINIUMDISTANCE __RANGEFINDERIOC(1)
|
||||
|
||||
/** set the maximum effective distance of the device */
|
||||
#define RANGEFINDERIOCSETMAXIUMDISTANCE __RANGEFINDERIOC(2)
|
||||
|
||||
|
||||
#endif /* _DRV_RANGEFINDER_H */
|
||||
|
|
@ -0,0 +1,42 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (C) 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the Maxbotix Sonar driver.
|
||||
#
|
||||
|
||||
APPNAME = mb12xx
|
||||
PRIORITY = SCHED_PRIORITY_DEFAULT
|
||||
STACKSIZE = 2048
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
|
|
@ -0,0 +1,842 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 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 mb12xx.cpp
|
||||
* @author Greg Hulands
|
||||
*
|
||||
* Driver for the Maxbotix sonar range finders connected via I2C.
|
||||
*/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
#include <sys/types.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <stdbool.h>
|
||||
#include <semaphore.h>
|
||||
#include <string.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <errno.h>
|
||||
#include <stdio.h>
|
||||
#include <math.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/wqueue.h>
|
||||
#include <nuttx/clock.h>
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <systemlib/perf_counter.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_range_finder.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
||||
/* Configuration Constants */
|
||||
#define MB12XX_BUS PX4_I2C_BUS_EXPANSION
|
||||
#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */
|
||||
|
||||
/* MB12xx Registers addresses */
|
||||
|
||||
#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */
|
||||
#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */
|
||||
#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */
|
||||
|
||||
/* Device limits */
|
||||
#define MB12XX_MIN_DISTANCE (0.20f)
|
||||
#define MB12XX_MAX_DISTANCE (7.65f)
|
||||
|
||||
#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
#ifndef CONFIG_SCHED_WORKQUEUE
|
||||
# error This requires CONFIG_SCHED_WORKQUEUE.
|
||||
#endif
|
||||
|
||||
class MB12XX : public device::I2C
|
||||
{
|
||||
public:
|
||||
MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR);
|
||||
~MB12XX();
|
||||
|
||||
virtual int init();
|
||||
|
||||
virtual ssize_t read(struct file *filp, char *buffer, size_t buflen);
|
||||
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
|
||||
|
||||
/**
|
||||
* Diagnostics - print some basic information about the driver.
|
||||
*/
|
||||
void print_info();
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
|
||||
private:
|
||||
float _min_distance;
|
||||
float _max_distance;
|
||||
work_s _work;
|
||||
unsigned _num_reports;
|
||||
volatile unsigned _next_report;
|
||||
volatile unsigned _oldest_report;
|
||||
range_finder_report *_reports;
|
||||
bool _sensor_ok;
|
||||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
|
||||
orb_advert_t _range_finder_topic;
|
||||
|
||||
perf_counter_t _sample_perf;
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _buffer_overflows;
|
||||
|
||||
/**
|
||||
* Test whether the device supported by the driver is present at a
|
||||
* specific address.
|
||||
*
|
||||
* @param address The I2C bus address to probe.
|
||||
* @return True if the device is present.
|
||||
*/
|
||||
int probe_address(uint8_t address);
|
||||
|
||||
/**
|
||||
* Initialise the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* Stop the automatic measurement state machine.
|
||||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Set the min and max distance thresholds if you want the end points of the sensors
|
||||
* range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE
|
||||
* and MB12XX_MAX_DISTANCE
|
||||
*/
|
||||
void set_minimum_distance(float min);
|
||||
void set_maximum_distance(float max);
|
||||
float get_minimum_distance();
|
||||
float get_maximum_distance();
|
||||
|
||||
/**
|
||||
* Perform a poll cycle; collect from the previous measurement
|
||||
* and start a new one.
|
||||
*/
|
||||
void cycle();
|
||||
int measure();
|
||||
int collect();
|
||||
/**
|
||||
* Static trampoline from the workq context; because we don't have a
|
||||
* generic workq wrapper yet.
|
||||
*
|
||||
* @param arg Instance pointer for the driver that is polling.
|
||||
*/
|
||||
static void cycle_trampoline(void *arg);
|
||||
|
||||
|
||||
};
|
||||
|
||||
/* helper macro for handling report buffer indices */
|
||||
#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0)
|
||||
|
||||
/*
|
||||
* Driver 'main' command.
|
||||
*/
|
||||
extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]);
|
||||
|
||||
MB12XX::MB12XX(int bus, int address) :
|
||||
I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000),
|
||||
_min_distance(MB12XX_MIN_DISTANCE),
|
||||
_max_distance(MB12XX_MAX_DISTANCE),
|
||||
_num_reports(0),
|
||||
_next_report(0),
|
||||
_oldest_report(0),
|
||||
_reports(nullptr),
|
||||
_sensor_ok(false),
|
||||
_measure_ticks(0),
|
||||
_collect_phase(false),
|
||||
_range_finder_topic(-1),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
|
||||
// work_cancel in the dtor will explode if we don't do this...
|
||||
memset(&_work, 0, sizeof(_work));
|
||||
}
|
||||
|
||||
MB12XX::~MB12XX()
|
||||
{
|
||||
/* make sure we are truly inactive */
|
||||
stop();
|
||||
|
||||
/* free any existing reports */
|
||||
if (_reports != nullptr)
|
||||
delete[] _reports;
|
||||
}
|
||||
|
||||
int
|
||||
MB12XX::init()
|
||||
{
|
||||
int ret = ERROR;
|
||||
|
||||
/* do I2C init (and probe) first */
|
||||
if (I2C::init() != OK)
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_num_reports = 2;
|
||||
_reports = new struct range_finder_report[_num_reports];
|
||||
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* get a publish handle on the range finder topic */
|
||||
memset(&_reports[0], 0, sizeof(_reports[0]));
|
||||
_range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]);
|
||||
|
||||
if (_range_finder_topic < 0)
|
||||
debug("failed to create sensor_range_finder object. Did you start uOrb?");
|
||||
|
||||
ret = OK;
|
||||
/* sensor is ok, but we don't really know if it is within range */
|
||||
_sensor_ok = true;
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
MB12XX::probe()
|
||||
{
|
||||
// TODO: take a range reading and see if it is between the min and max
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::set_minimum_distance(float min)
|
||||
{
|
||||
_min_distance = min;
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::set_maximum_distance(float max)
|
||||
{
|
||||
_max_distance = max;
|
||||
}
|
||||
|
||||
float
|
||||
MB12XX::get_minimum_distance()
|
||||
{
|
||||
return _min_distance;
|
||||
}
|
||||
|
||||
float
|
||||
MB12XX::get_maximum_distance()
|
||||
{
|
||||
return _max_distance;
|
||||
}
|
||||
|
||||
int
|
||||
MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
|
||||
case SENSORIOCSPOLLRATE: {
|
||||
switch (arg) {
|
||||
|
||||
/* switching to manual polling */
|
||||
case SENSOR_POLLRATE_MANUAL:
|
||||
stop();
|
||||
_measure_ticks = 0;
|
||||
return OK;
|
||||
|
||||
/* external signalling (DRDY) not supported */
|
||||
case SENSOR_POLLRATE_EXTERNAL:
|
||||
|
||||
/* zero would be bad */
|
||||
case 0:
|
||||
return -EINVAL;
|
||||
|
||||
/* set default/max polling rate */
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* set interval for next measurement to minimum legal value */
|
||||
_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
/* do we need to start internal polling? */
|
||||
bool want_start = (_measure_ticks == 0);
|
||||
|
||||
/* convert hz to tick interval via microseconds */
|
||||
unsigned ticks = USEC2TICK(1000000 / arg);
|
||||
|
||||
/* check against maximum rate */
|
||||
if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
|
||||
return -EINVAL;
|
||||
|
||||
/* update interval for next measurement */
|
||||
_measure_ticks = ticks;
|
||||
|
||||
/* if we need to start the poll state machine, do it */
|
||||
if (want_start)
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
case SENSORIOCGPOLLRATE:
|
||||
if (_measure_ticks == 0)
|
||||
return SENSOR_POLLRATE_MANUAL;
|
||||
|
||||
return (1000 / _measure_ticks);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* add one to account for the sentinel in the ring */
|
||||
arg++;
|
||||
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 2) || (arg > 100))
|
||||
return -EINVAL;
|
||||
|
||||
/* allocate new buffer */
|
||||
struct range_finder_report *buf = new struct range_finder_report[arg];
|
||||
|
||||
if (nullptr == buf)
|
||||
return -ENOMEM;
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
delete[] _reports;
|
||||
_num_reports = arg;
|
||||
_reports = buf;
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
return _num_reports - 1;
|
||||
|
||||
case SENSORIOCRESET:
|
||||
/* XXX implement this */
|
||||
return -EINVAL;
|
||||
|
||||
case RANGEFINDERIOCSETMINIUMDISTANCE:
|
||||
{
|
||||
set_minimum_distance(*(float *)arg);
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
case RANGEFINDERIOCSETMAXIUMDISTANCE:
|
||||
{
|
||||
set_maximum_distance(*(float *)arg);
|
||||
return 0;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
/* give it to the superclass */
|
||||
return I2C::ioctl(filp, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
ssize_t
|
||||
MB12XX::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct range_finder_report);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (count < 1)
|
||||
return -ENOSPC;
|
||||
|
||||
/* if automatic measurement is enabled */
|
||||
if (_measure_ticks > 0) {
|
||||
|
||||
/*
|
||||
* While there is space in the caller's buffer, and reports, copy them.
|
||||
* Note that we may be pre-empted by the workq thread while we are doing this;
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_oldest_report != _next_report) {
|
||||
memcpy(buffer, _reports + _oldest_report, sizeof(*_reports));
|
||||
ret += sizeof(_reports[0]);
|
||||
INCREMENT(_oldest_report, _num_reports);
|
||||
}
|
||||
}
|
||||
|
||||
/* if there was no data, warn the caller */
|
||||
return ret ? ret : -EAGAIN;
|
||||
}
|
||||
|
||||
/* manual measurement - run one conversion */
|
||||
/* XXX really it'd be nice to lock against other readers here */
|
||||
do {
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* trigger a measurement */
|
||||
if (OK != measure()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* wait for it to complete */
|
||||
usleep(MB12XX_CONVERSION_INTERVAL);
|
||||
|
||||
/* run the collection phase */
|
||||
if (OK != collect()) {
|
||||
ret = -EIO;
|
||||
break;
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
memcpy(buffer, _reports, sizeof(*_reports));
|
||||
ret = sizeof(*_reports);
|
||||
|
||||
} while (0);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
MB12XX::measure()
|
||||
{
|
||||
int ret;
|
||||
|
||||
/*
|
||||
* Send the command to begin a measurement.
|
||||
*/
|
||||
uint8_t cmd = MB12XX_TAKE_RANGE_REG;
|
||||
ret = transfer(&cmd, 1, nullptr, 0);
|
||||
|
||||
if (OK != ret)
|
||||
{
|
||||
perf_count(_comms_errors);
|
||||
log("i2c::transfer returned %d", ret);
|
||||
return ret;
|
||||
}
|
||||
ret = OK;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
MB12XX::collect()
|
||||
{
|
||||
int ret = -EIO;
|
||||
|
||||
/* read from the sensor */
|
||||
uint8_t val[2] = {0, 0};
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
ret = transfer(nullptr, 0, &val[0], 2);
|
||||
|
||||
if (ret < 0)
|
||||
{
|
||||
log("error reading from sensor: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint16_t distance = val[0] << 8 | val[1];
|
||||
float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */
|
||||
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
|
||||
_reports[_next_report].timestamp = hrt_absolute_time();
|
||||
_reports[_next_report].distance = si_units;
|
||||
_reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0;
|
||||
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]);
|
||||
|
||||
/* post a report to the ring - note, not locked */
|
||||
INCREMENT(_next_report, _num_reports);
|
||||
|
||||
/* if we are running up against the oldest report, toss it */
|
||||
if (_next_report == _oldest_report) {
|
||||
perf_count(_buffer_overflows);
|
||||
INCREMENT(_oldest_report, _num_reports);
|
||||
}
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
ret = OK;
|
||||
|
||||
out:
|
||||
perf_end(_sample_perf);
|
||||
return ret;
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::start()
|
||||
{
|
||||
/* reset the report ring and state machine */
|
||||
_collect_phase = false;
|
||||
_oldest_report = _next_report = 0;
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1);
|
||||
|
||||
/* notify about state change */
|
||||
struct subsystem_info_s info = {
|
||||
true,
|
||||
true,
|
||||
true,
|
||||
SUBSYSTEM_TYPE_RANGEFINDER};
|
||||
static orb_advert_t pub = -1;
|
||||
|
||||
if (pub > 0) {
|
||||
orb_publish(ORB_ID(subsystem_info), pub, &info);
|
||||
} else {
|
||||
pub = orb_advertise(ORB_ID(subsystem_info), &info);
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::stop()
|
||||
{
|
||||
work_cancel(HPWORK, &_work);
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::cycle_trampoline(void *arg)
|
||||
{
|
||||
MB12XX *dev = (MB12XX *)arg;
|
||||
|
||||
dev->cycle();
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::cycle()
|
||||
{
|
||||
/* collection phase? */
|
||||
if (_collect_phase) {
|
||||
|
||||
/* perform collection */
|
||||
if (OK != collect()) {
|
||||
log("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
/* next phase is measurement */
|
||||
_collect_phase = false;
|
||||
|
||||
/*
|
||||
* Is there a collect->measure gap?
|
||||
*/
|
||||
if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) {
|
||||
|
||||
/* schedule a fresh cycle call when we are ready to measure again */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&MB12XX::cycle_trampoline,
|
||||
this,
|
||||
_measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL));
|
||||
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* measurement phase */
|
||||
if (OK != measure())
|
||||
log("measure error");
|
||||
|
||||
/* next phase is collection */
|
||||
_collect_phase = true;
|
||||
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
work_queue(HPWORK,
|
||||
&_work,
|
||||
(worker_t)&MB12XX::cycle_trampoline,
|
||||
this,
|
||||
USEC2TICK(MB12XX_CONVERSION_INTERVAL));
|
||||
}
|
||||
|
||||
void
|
||||
MB12XX::print_info()
|
||||
{
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
perf_print_counter(_buffer_overflows);
|
||||
printf("poll interval: %u ticks\n", _measure_ticks);
|
||||
printf("report queue: %u (%u/%u @ %p)\n",
|
||||
_num_reports, _oldest_report, _next_report, _reports);
|
||||
}
|
||||
|
||||
/**
|
||||
* Local functions in support of the shell command.
|
||||
*/
|
||||
namespace mb12xx
|
||||
{
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
const int ERROR = -1;
|
||||
|
||||
MB12XX *g_dev;
|
||||
|
||||
void start();
|
||||
void stop();
|
||||
void test();
|
||||
void reset();
|
||||
void info();
|
||||
|
||||
/**
|
||||
* Start the driver.
|
||||
*/
|
||||
void
|
||||
start()
|
||||
{
|
||||
int fd;
|
||||
|
||||
if (g_dev != nullptr)
|
||||
errx(1, "already started");
|
||||
|
||||
/* create the driver */
|
||||
g_dev = new MB12XX(MB12XX_BUS);
|
||||
|
||||
if (g_dev == nullptr)
|
||||
goto fail;
|
||||
|
||||
if (OK != g_dev->init())
|
||||
goto fail;
|
||||
|
||||
/* set the poll rate to default, starts automatic data collection */
|
||||
fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
goto fail;
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
goto fail;
|
||||
|
||||
exit(0);
|
||||
|
||||
fail:
|
||||
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
|
||||
errx(1, "driver start failed");
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the driver
|
||||
*/
|
||||
void stop()
|
||||
{
|
||||
if (g_dev != nullptr)
|
||||
{
|
||||
delete g_dev;
|
||||
g_dev = nullptr;
|
||||
}
|
||||
else
|
||||
{
|
||||
errx(1, "driver not running");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Perform some basic functional tests on the driver;
|
||||
* make sure we can collect data from the sensor in polled
|
||||
* and automatic modes.
|
||||
*/
|
||||
void
|
||||
test()
|
||||
{
|
||||
struct range_finder_report report;
|
||||
ssize_t sz;
|
||||
int ret;
|
||||
|
||||
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH);
|
||||
|
||||
/* do a simple demand read */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
err(1, "immediate read failed");
|
||||
|
||||
warnx("single read");
|
||||
warnx("measurement: %0.2f m", (double)report.distance);
|
||||
warnx("time: %lld", report.timestamp);
|
||||
|
||||
/* start the sensor polling at 2Hz */
|
||||
if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2))
|
||||
errx(1, "failed to set 2Hz poll rate");
|
||||
|
||||
/* read the sensor 5x and report each value */
|
||||
for (unsigned i = 0; i < 5; i++) {
|
||||
struct pollfd fds;
|
||||
|
||||
/* wait for data to be ready */
|
||||
fds.fd = fd;
|
||||
fds.events = POLLIN;
|
||||
ret = poll(&fds, 1, 2000);
|
||||
|
||||
if (ret != 1)
|
||||
errx(1, "timed out waiting for sensor data");
|
||||
|
||||
/* now go get it */
|
||||
sz = read(fd, &report, sizeof(report));
|
||||
|
||||
if (sz != sizeof(report))
|
||||
err(1, "periodic read failed");
|
||||
|
||||
warnx("periodic read %u", i);
|
||||
warnx("measurement: %0.3f", (double)report.distance);
|
||||
warnx("time: %lld", report.timestamp);
|
||||
}
|
||||
|
||||
errx(0, "PASS");
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the driver.
|
||||
*/
|
||||
void
|
||||
reset()
|
||||
{
|
||||
int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "failed ");
|
||||
|
||||
if (ioctl(fd, SENSORIOCRESET, 0) < 0)
|
||||
err(1, "driver reset failed");
|
||||
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0)
|
||||
err(1, "driver poll restart failed");
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Print a little info about the driver.
|
||||
*/
|
||||
void
|
||||
info()
|
||||
{
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "driver not running");
|
||||
|
||||
printf("state @ %p\n", g_dev);
|
||||
g_dev->print_info();
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int
|
||||
mb12xx_main(int argc, char *argv[])
|
||||
{
|
||||
/*
|
||||
* Start/load the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "start"))
|
||||
mb12xx::start();
|
||||
|
||||
/*
|
||||
* Stop the driver
|
||||
*/
|
||||
if (!strcmp(argv[1], "stop"))
|
||||
mb12xx::stop();
|
||||
|
||||
/*
|
||||
* Test the driver/device.
|
||||
*/
|
||||
if (!strcmp(argv[1], "test"))
|
||||
mb12xx::test();
|
||||
|
||||
/*
|
||||
* Reset the driver.
|
||||
*/
|
||||
if (!strcmp(argv[1], "reset"))
|
||||
mb12xx::reset();
|
||||
|
||||
/*
|
||||
* Print driver information.
|
||||
*/
|
||||
if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status"))
|
||||
mb12xx::info();
|
||||
|
||||
errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
|
||||
}
|
||||
|
|
@ -82,7 +82,9 @@
|
|||
#include <uORB/topics/parameter_update.h>
|
||||
|
||||
#include <px4io/protocol.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include "uploader.h"
|
||||
#include <debug.h>
|
||||
|
||||
|
||||
class PX4IO : public device::I2C
|
||||
|
|
@ -111,6 +113,8 @@ private:
|
|||
volatile int _task; ///< worker task
|
||||
volatile bool _task_should_exit;
|
||||
|
||||
int _mavlink_fd;
|
||||
|
||||
perf_counter_t _perf_update;
|
||||
|
||||
/* cached IO state */
|
||||
|
|
@ -285,6 +289,7 @@ PX4IO::PX4IO() :
|
|||
_update_interval(0),
|
||||
_task(-1),
|
||||
_task_should_exit(false),
|
||||
_mavlink_fd(-1),
|
||||
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
|
||||
_status(0),
|
||||
_alarms(0),
|
||||
|
|
@ -301,6 +306,9 @@ PX4IO::PX4IO() :
|
|||
/* we need this potentially before it could be set in task_main */
|
||||
g_dev = this;
|
||||
|
||||
/* open MAVLink text channel */
|
||||
_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
|
||||
|
||||
_debug_enabled = true;
|
||||
}
|
||||
|
||||
|
|
@ -354,6 +362,7 @@ PX4IO::init()
|
|||
(_max_rc_input < 1) || (_max_rc_input > 255)) {
|
||||
|
||||
log("failed getting parameters from PX4IO");
|
||||
mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort.");
|
||||
return -1;
|
||||
}
|
||||
if (_max_rc_input > RC_INPUT_MAX_CHANNELS)
|
||||
|
|
@ -380,6 +389,8 @@ PX4IO::init()
|
|||
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
|
||||
(reg & PX4IO_P_SETUP_ARMING_ARM_OK)) {
|
||||
|
||||
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
|
||||
|
||||
/* WARNING: COMMANDER app/vehicle status must be initialized.
|
||||
* If this fails (or the app is not started), worst-case IO
|
||||
* remains untouched (so manual override is still available).
|
||||
|
|
@ -469,6 +480,7 @@ PX4IO::init()
|
|||
ret = io_set_rc_config();
|
||||
if (ret != OK) {
|
||||
log("failed to update RC input config");
|
||||
mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail");
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
|
@ -490,6 +502,8 @@ PX4IO::init()
|
|||
return -errno;
|
||||
}
|
||||
|
||||
mavlink_log_info(_mavlink_fd, "[IO] init ok");
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
|
@ -771,9 +785,16 @@ PX4IO::io_set_rc_config()
|
|||
/* send channel config to IO */
|
||||
ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
|
||||
if (ret != OK) {
|
||||
log("RC config update failed");
|
||||
log("rc config upload failed");
|
||||
break;
|
||||
}
|
||||
|
||||
/* check the IO initialisation flag */
|
||||
if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
|
||||
log("config for RC%d rejected by IO", i + 1);
|
||||
break;
|
||||
}
|
||||
|
||||
offset += PX4IO_P_RC_CONFIG_STRIDE;
|
||||
}
|
||||
|
||||
|
|
@ -1157,9 +1178,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
|
|||
/* check for the mixer-OK flag */
|
||||
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) {
|
||||
debug("mixer upload OK");
|
||||
mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok");
|
||||
return 0;
|
||||
} else {
|
||||
debug("mixer rejected by IO");
|
||||
mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
|
||||
}
|
||||
|
||||
/* load must have failed for some reason */
|
||||
|
|
@ -1186,7 +1209,7 @@ PX4IO::print_status()
|
|||
printf("%u bytes free\n",
|
||||
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
|
||||
uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
|
||||
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
|
||||
printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n",
|
||||
flags,
|
||||
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
|
||||
|
|
@ -1197,7 +1220,8 @@ PX4IO::print_status()
|
|||
((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"));
|
||||
((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"),
|
||||
((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"));
|
||||
uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS);
|
||||
printf("alarms 0x%04x%s%s%s%s%s%s\n",
|
||||
alarms,
|
||||
|
|
@ -1475,7 +1499,7 @@ test(void)
|
|||
servos[i] = pwm_value;
|
||||
|
||||
ret = write(fd, servos, sizeof(servos));
|
||||
if (ret != sizeof(servos))
|
||||
if (ret != (int)sizeof(servos))
|
||||
err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret);
|
||||
|
||||
if (direction > 0) {
|
||||
|
|
@ -1547,7 +1571,7 @@ px4io_main(int argc, char *argv[])
|
|||
errx(1, "already loaded");
|
||||
|
||||
/* create the driver - it will set g_dev */
|
||||
(void)new PX4IO;
|
||||
(void)new PX4IO();
|
||||
|
||||
if (g_dev == nullptr)
|
||||
errx(1, "driver alloc failed");
|
||||
|
|
@ -1558,7 +1582,7 @@ px4io_main(int argc, char *argv[])
|
|||
}
|
||||
|
||||
/* look for the optional pwm update rate for the supported modes */
|
||||
if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) {
|
||||
if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) {
|
||||
if (argc > 2 + 1) {
|
||||
#warning implement this
|
||||
} else {
|
||||
|
|
@ -1570,16 +1594,31 @@ px4io_main(int argc, char *argv[])
|
|||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "recovery")) {
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
/*
|
||||
* Enable in-air restart support.
|
||||
* We can cheat and call the driver directly, as it
|
||||
* doesn't reference filp in ioctl()
|
||||
*/
|
||||
g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0);
|
||||
} else {
|
||||
errx(1, "not loaded");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
if (!strcmp(argv[1], "stop")) {
|
||||
|
||||
if (g_dev != nullptr) {
|
||||
/* stop the driver */
|
||||
delete g_dev;
|
||||
} else {
|
||||
errx(1, "not loaded");
|
||||
}
|
||||
exit(0);
|
||||
if (g_dev != nullptr) {
|
||||
/* stop the driver */
|
||||
delete g_dev;
|
||||
} else {
|
||||
errx(1, "not loaded");
|
||||
}
|
||||
exit(0);
|
||||
}
|
||||
|
||||
|
||||
if (!strcmp(argv[1], "status")) {
|
||||
|
|
@ -1604,8 +1643,9 @@ px4io_main(int argc, char *argv[])
|
|||
exit(1);
|
||||
}
|
||||
uint8_t level = atoi(argv[2]);
|
||||
// we can cheat and call the driver directly, as it
|
||||
// doesn't reference filp in ioctl()
|
||||
/* we can cheat and call the driver directly, as it
|
||||
* doesn't reference filp in ioctl()
|
||||
*/
|
||||
int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level);
|
||||
if (ret != 0) {
|
||||
printf("SET_DEBUG failed - %d\n", ret);
|
||||
|
|
|
|||
|
|
@ -63,7 +63,11 @@
|
|||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||
* @param _text The text to log;
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
|
||||
#else
|
||||
#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Send a mavlink critical message.
|
||||
|
|
@ -71,7 +75,11 @@
|
|||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||
* @param _text The text to log;
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
|
||||
#else
|
||||
#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Send a mavlink info message.
|
||||
|
|
@ -79,7 +87,11 @@
|
|||
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
|
||||
* @param _text The text to log;
|
||||
*/
|
||||
#ifdef __cplusplus
|
||||
#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
|
||||
#else
|
||||
#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
|
||||
#endif
|
||||
|
||||
struct mavlink_logmessage {
|
||||
char text[51];
|
||||
|
|
|
|||
|
|
@ -147,32 +147,44 @@ controls_tick() {
|
|||
|
||||
uint16_t raw = r_raw_rc_values[i];
|
||||
|
||||
/* implement the deadzone */
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) {
|
||||
raw += conf[PX4IO_P_RC_CONFIG_DEADZONE];
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_CENTER])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_CENTER];
|
||||
}
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) {
|
||||
raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE];
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_CENTER])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_CENTER];
|
||||
}
|
||||
int16_t scaled;
|
||||
|
||||
/* constrain to min/max values */
|
||||
/*
|
||||
* 1) Constrain to min/max values, as later processing depends on bounds.
|
||||
*/
|
||||
if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MIN];
|
||||
if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
|
||||
raw = conf[PX4IO_P_RC_CONFIG_MAX];
|
||||
|
||||
int16_t scaled = raw;
|
||||
/*
|
||||
* 2) Scale around the mid point differently for lower and upper range.
|
||||
*
|
||||
* This is necessary as they don't share the same endpoints and slope.
|
||||
*
|
||||
* First normalize to 0..1 range with correct sign (below or above center),
|
||||
* then scale to 20000 range (if center is an actual center, -10000..10000,
|
||||
* if parameters only support half range, scale to 10000 range, e.g. if
|
||||
* center == min 0..10000, if center == max -10000..0).
|
||||
*
|
||||
* As the min and max bounds were enforced in step 1), division by zero
|
||||
* cannot occur, as for the case of center == min or center == max the if
|
||||
* statement is mutually exclusive with the arithmetic NaN case.
|
||||
*
|
||||
* DO NOT REMOVE OR ALTER STEP 1!
|
||||
*/
|
||||
if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
|
||||
|
||||
/* adjust to zero-relative around center (nominal -500..500) */
|
||||
scaled -= conf[PX4IO_P_RC_CONFIG_CENTER];
|
||||
} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
|
||||
|
||||
/* scale to fixed-point representation (-10000..10000) */
|
||||
scaled *= 20;
|
||||
} else {
|
||||
/* in the configured dead zone, output zero */
|
||||
scaled = 0;
|
||||
}
|
||||
|
||||
/* invert channel if requested */
|
||||
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
|
||||
scaled = -scaled;
|
||||
|
||||
|
|
@ -256,10 +268,11 @@ controls_tick() {
|
|||
bool override = false;
|
||||
|
||||
/*
|
||||
* Check mapped channel 5; if the value is 'high' then the pilot has
|
||||
* Check mapped channel 5 (can be any remote channel,
|
||||
* depends on RC_MAP_OVER parameter);
|
||||
* If the value is 'high' then the pilot has
|
||||
* requested override.
|
||||
*
|
||||
* XXX This should be configurable.
|
||||
*/
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH))
|
||||
override = true;
|
||||
|
|
|
|||
|
|
@ -109,9 +109,11 @@ mixer_tick(void)
|
|||
/*
|
||||
* Decide which set of controls we're using.
|
||||
*/
|
||||
if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) {
|
||||
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ||
|
||||
!(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
|
||||
|
||||
/* don't actually mix anything - we already have raw PWM values */
|
||||
/* don't actually mix anything - we already have raw PWM values or
|
||||
not a valid mixer. */
|
||||
source = MIX_NONE;
|
||||
|
||||
} else {
|
||||
|
|
@ -172,9 +174,11 @@ mixer_tick(void)
|
|||
* XXX correct behaviour for failsafe may require an additional case
|
||||
* here.
|
||||
*/
|
||||
bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
|
||||
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)));
|
||||
bool should_arm = (
|
||||
/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) &&
|
||||
/* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) &&
|
||||
/* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK));
|
||||
|
||||
if (should_arm && !mixer_servos_armed) {
|
||||
/* need to arm, but not armed */
|
||||
|
|
@ -239,6 +243,11 @@ static unsigned mixer_text_length = 0;
|
|||
void
|
||||
mixer_handle_text(const void *buffer, size_t length)
|
||||
{
|
||||
/* do not allow a mixer change while fully armed */
|
||||
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
return;
|
||||
}
|
||||
|
||||
px4io_mixdata *msg = (px4io_mixdata *)buffer;
|
||||
|
||||
|
|
@ -252,9 +261,12 @@ mixer_handle_text(const void *buffer, size_t length)
|
|||
switch (msg->action) {
|
||||
case F2I_MIXER_ACTION_RESET:
|
||||
isr_debug(2, "reset");
|
||||
|
||||
/* FIRST mark the mixer as invalid */
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||
/* THEN actually delete it */
|
||||
mixer_group.reset();
|
||||
mixer_text_length = 0;
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
|
||||
|
||||
/* FALLTHROUGH */
|
||||
case F2I_MIXER_ACTION_APPEND:
|
||||
|
|
|
|||
|
|
@ -103,6 +103,7 @@
|
|||
#define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */
|
||||
#define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */
|
||||
#define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */
|
||||
#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */
|
||||
|
||||
#define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */
|
||||
#define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */
|
||||
|
|
|
|||
|
|
@ -215,12 +215,16 @@ user_start(int argc, char *argv[])
|
|||
|
||||
/* post debug state at ~1Hz */
|
||||
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
|
||||
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u",
|
||||
|
||||
struct mallinfo minfo = mallinfo();
|
||||
|
||||
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u",
|
||||
(unsigned)debug_level,
|
||||
(unsigned)r_status_flags,
|
||||
(unsigned)r_setup_arming,
|
||||
(unsigned)r_setup_features,
|
||||
(unsigned)i2c_loop_resets);
|
||||
(unsigned)i2c_loop_resets,
|
||||
(unsigned)minfo.mxordblk);
|
||||
last_debug_time = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -361,6 +361,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
break;
|
||||
|
||||
case PX4IO_PAGE_RC_CONFIG: {
|
||||
|
||||
/* do not allow a RC config change while fully armed */
|
||||
if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) &&
|
||||
/* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
|
||||
break;
|
||||
}
|
||||
|
||||
unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE;
|
||||
unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE;
|
||||
uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE];
|
||||
|
|
@ -383,6 +390,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
|
||||
case PX4IO_P_RC_CONFIG_OPTIONS:
|
||||
value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID;
|
||||
r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
|
||||
/* set all options except the enabled option */
|
||||
conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
|
|
@ -390,27 +398,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
|
|||
/* should the channel be enabled? */
|
||||
/* this option is normally set last */
|
||||
if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
|
||||
uint8_t count = 0;
|
||||
|
||||
/* assert min..center..max ordering */
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500)
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500)
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN])
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX])
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) {
|
||||
count++;
|
||||
}
|
||||
if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) {
|
||||
count++;
|
||||
}
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) {
|
||||
count++;
|
||||
}
|
||||
if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) {
|
||||
count++;
|
||||
}
|
||||
|
||||
/* assert deadzone is sane */
|
||||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500)
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]))
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]))
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS)
|
||||
break;
|
||||
if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) {
|
||||
count++;
|
||||
}
|
||||
// The following check isn't needed as constraint checks in controls.c will catch this.
|
||||
//if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
// count++;
|
||||
//}
|
||||
//if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
|
||||
// count++;
|
||||
//}
|
||||
if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
|
||||
count++;
|
||||
}
|
||||
|
||||
/* sanity checks pass, enable channel */
|
||||
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
if (count) {
|
||||
isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1));
|
||||
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK;
|
||||
} else {
|
||||
conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
|
||||
}
|
||||
}
|
||||
break;
|
||||
/* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */
|
||||
|
|
|
|||
|
|
@ -176,12 +176,17 @@ heartbeat_blink(void *arg)
|
|||
static void
|
||||
failsafe_blink(void *arg)
|
||||
{
|
||||
/* indicate that a serious initialisation error occured */
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) {
|
||||
LED_AMBER(true);
|
||||
return;
|
||||
}
|
||||
|
||||
static bool failsafe = false;
|
||||
|
||||
/* blink the failsafe LED if we don't have FMU input */
|
||||
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
|
||||
failsafe = !failsafe;
|
||||
|
||||
} else {
|
||||
failsafe = false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1129,31 +1129,45 @@ Sensors::ppm_poll()
|
|||
/* Read out values from raw message */
|
||||
for (unsigned int i = 0; i < channel_limit; i++) {
|
||||
|
||||
/* scale around the mid point differently for lower and upper range */
|
||||
/*
|
||||
* 1) Constrain to min/max values, as later processing depends on bounds.
|
||||
*/
|
||||
if (rc_input.values[i] < _parameters.min[i])
|
||||
rc_input.values[i] = _parameters.min[i];
|
||||
if (rc_input.values[i] > _parameters.max[i])
|
||||
rc_input.values[i] = _parameters.max[i];
|
||||
|
||||
/*
|
||||
* 2) Scale around the mid point differently for lower and upper range.
|
||||
*
|
||||
* This is necessary as they don't share the same endpoints and slope.
|
||||
*
|
||||
* First normalize to 0..1 range with correct sign (below or above center),
|
||||
* the total range is 2 (-1..1).
|
||||
* If center (trim) == min, scale to 0..1, if center (trim) == max,
|
||||
* scale to -1..0.
|
||||
*
|
||||
* As the min and max bounds were enforced in step 1), division by zero
|
||||
* cannot occur, as for the case of center == min or center == max the if
|
||||
* statement is mutually exclusive with the arithmetic NaN case.
|
||||
*
|
||||
* DO NOT REMOVE OR ALTER STEP 1!
|
||||
*/
|
||||
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
|
||||
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
|
||||
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
|
||||
|
||||
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
|
||||
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
|
||||
_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
|
||||
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
|
||||
|
||||
} else {
|
||||
/* in the configured dead zone, output zero */
|
||||
_rc.chan[i].scaled = 0.0f;
|
||||
}
|
||||
|
||||
/* reverse channel if required */
|
||||
if (i == (int)_rc.function[THROTTLE]) {
|
||||
if ((int)_parameters.rev[i] == -1) {
|
||||
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
|
||||
}
|
||||
|
||||
} else {
|
||||
_rc.chan[i].scaled *= _parameters.rev[i];
|
||||
}
|
||||
_rc.chan[i].scaled *= _parameters.rev[i];
|
||||
|
||||
/* handle any parameter-induced blowups */
|
||||
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
|
||||
if (!isfinite(_rc.chan[i].scaled))
|
||||
_rc.chan[i].scaled = 0.0f;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -117,7 +117,23 @@ load(const char *devname, const char *fname)
|
|||
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
|
||||
continue;
|
||||
|
||||
/* XXX an optimisation here would be to strip extra whitespace */
|
||||
/* compact whitespace in the buffer */
|
||||
char *t, *f;
|
||||
for (f = buf; *f != '\0'; f++) {
|
||||
/* scan for space characters */
|
||||
if (*f == ' ') {
|
||||
/* look for additional spaces */
|
||||
t = f + 1;
|
||||
while (*t == ' ')
|
||||
t++;
|
||||
if (*t == '\0') {
|
||||
/* strip trailing whitespace */
|
||||
*f = '\0';
|
||||
} else if (t > (f + 1)) {
|
||||
memmove(f + 1, t, strlen(t) + 1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* if the line is too long to fit in the buffer, bail */
|
||||
if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf))
|
||||
|
|
|
|||
|
|
@ -54,6 +54,7 @@
|
|||
#include "mixer.h"
|
||||
|
||||
Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) :
|
||||
_next(nullptr),
|
||||
_control_cb(control_cb),
|
||||
_cb_handle(cb_handle)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -93,6 +93,7 @@ MixerGroup::reset()
|
|||
mixer = _first;
|
||||
_first = mixer->_next;
|
||||
delete mixer;
|
||||
mixer = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -53,6 +53,9 @@ ORB_DEFINE(sensor_gyro, struct gyro_report);
|
|||
#include <drivers/drv_baro.h>
|
||||
ORB_DEFINE(sensor_baro, struct baro_report);
|
||||
|
||||
#include <drivers/drv_range_finder.h>
|
||||
ORB_DEFINE(sensor_range_finder, struct range_finder_report);
|
||||
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
ORB_DEFINE(output_pwm, struct pwm_output_values);
|
||||
|
||||
|
|
|
|||
|
|
@ -71,7 +71,8 @@ enum SUBSYSTEM_TYPE
|
|||
SUBSYSTEM_TYPE_YAWPOSITION = 4096,
|
||||
SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384,
|
||||
SUBSYSTEM_TYPE_POSITIONCONTROL = 32768,
|
||||
SUBSYSTEM_TYPE_MOTORCONTROL = 65536
|
||||
SUBSYSTEM_TYPE_MOTORCONTROL = 65536,
|
||||
SUBSYSTEM_TYPE_RANGEFINDER = 131072
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -125,6 +125,7 @@ CONFIGURED_APPS += drivers/stm32/adc
|
|||
CONFIGURED_APPS += drivers/px4fmu
|
||||
CONFIGURED_APPS += drivers/hil
|
||||
CONFIGURED_APPS += drivers/gps
|
||||
CONFIGURED_APPS += drivers/mb12xx
|
||||
|
||||
# Testing stuff
|
||||
CONFIGURED_APPS += px4/sensors_bringup
|
||||
|
|
|
|||
Loading…
Reference in New Issue