fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 08:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into new_state_machine

Browse Source 
Conflicts:
	apps/commander/commander.c
	apps/uORB/topics/vehicle_status.h
This commit is contained in:
Julian Oes
2013-03-11 11:01:49 -07:00
parent 0fe5aeb02c 1d444f80a3
commit 591cc0ac4d
55 changed files with 2019 additions and 683 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Debug/ARMv7M
+108 -96
View File
@@ -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
ROMFS/logging/logconv.m
+2 -2
View File
@@ -33,7 +33,7 @@ end
% float vbat; //battery voltage in [volt]
% float bat_current - current drawn from battery at this time instant
% float bat_discharged - discharged energy in mAh
% float adc[3]; //remaining auxiliary ADC ports [volt]
% float adc[4]; //ADC ports [volt]
% float local_position[3]; //tangent plane mapping into x,y,z [m]
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
% float attitude[3]; //pitch, roll, yaw [rad]
@@ -57,7 +57,7 @@ logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, '
logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
apps/commander/commander.c
+119 -6
View File
@@ -76,6 +76,8 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -799,6 +801,72 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
close(sub_sensor_combined);
}
void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
{
/* announce change */
mavlink_log_info(mavlink_fd, "keep it still");
const int calibration_count = 2500;
int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
int calibration_counter = 0;
float airspeed_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
airspeed_offset += differential_pressure.voltage;
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
return;
}
}
airspeed_offset = airspeed_offset / calibration_count;
if (isfinite(airspeed_offset)) {
if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
//char buf[50];
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
}
close(sub_differential_pressure);
}
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
@@ -1018,6 +1086,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
handled = true;
}
/* airspeed calibration */
if ((int)(cmd->param6) == 1) {
if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
mavlink_log_info(mavlink_fd, "starting airspeed cal");
tune_confirm();
do_airspeed_calibration(status_pub, &current_status);
mavlink_log_info(mavlink_fd, "finished airspeed cal");
tune_confirm();
// XXX if this fails, go to ERROR
arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, mavlink_fd);
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
handled = true;
}
/* none found */
if (!handled) {
//warnx("refusing unsupported calibration request\n");
@@ -1438,6 +1528,11 @@ int commander_thread_main(int argc, char *argv[])
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
memset(&differential_pressure, 0, sizeof(differential_pressure));
uint64_t last_differential_pressure_time = 0;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
struct vehicle_command_s cmd;
@@ -1491,6 +1586,13 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
}
orb_check(differential_pressure_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
last_differential_pressure_time = differential_pressure.timestamp;
}
orb_check(cmd_sub, &new_data);
if (new_data) {
@@ -1691,6 +1793,8 @@ int commander_thread_main(int argc, char *argv[])
// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
bool global_pos_valid = current_status.condition_global_position_valid;
bool local_pos_valid = current_status.condition_local_position_valid;
bool airspeed_valid = current_status.condition_airspeed_valid;
/* check for global or local position updates, set a timeout of 2s */
if (hrt_absolute_time() - last_global_position_time < 2000000) {
@@ -1709,6 +1813,14 @@ int commander_thread_main(int argc, char *argv[])
current_status.condition_local_position_valid = false;
}
/* Check for valid airspeed/differential pressure measurements */
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
current_status.condition_airspeed_valid = true;
} else {
current_status.condition_airspeed_valid = false;
}
/*
* Consolidate global position and local position valid flags
* for vector flight mode.
@@ -1721,12 +1833,13 @@ int commander_thread_main(int argc, char *argv[])
// current_status.flag_vector_flight_mode_ok = false;
// }
// /* consolidate state change, flag as changed if required */
// if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
// global_pos_valid != current_status.flag_global_position_valid ||
// local_pos_valid != current_status.flag_local_position_valid) {
// state_changed = true;
// }
// XXX why is this needed?
/* consolidate state change, flag as changed if required */
if (global_pos_valid != current_status.condition_global_position_valid ||
local_pos_valid != current_status.condition_local_position_valid ||
airspeed_valid != current_status.condition_airspeed_valid) {
state_changed = true;
}
/*
* Mark the position of the first position lock as return to launch (RTL)
apps/controllib/fixedwing.cpp
+1 -1
View File
@@ -325,7 +325,7 @@ void BlockMultiModeBacksideAutopilot::update()
_att.roll, _att.pitch, _att.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed
);
_actuators.control[CH_AIL] = - _backsideAutopilot.getAileron();
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = - _backsideAutopilot.getElevator();
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
apps/drivers/blinkm/blinkm.cpp
+1 -1
View File
@@ -127,7 +127,7 @@ class BlinkM : public device::I2C
{
public:
BlinkM(int bus, int blinkm);
~BlinkM();
virtual ~BlinkM();
virtual int init();
apps/drivers/bma180/bma180.cpp
+1 -1
View File
@@ -126,7 +126,7 @@ class BMA180 : public device::SPI
{
public:
BMA180(int bus, spi_dev_e device);
~BMA180();
virtual ~BMA180();
virtual int init();
apps/drivers/drv_hrt.h
+16
View File
@@ -91,6 +91,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
*/
__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
/*
* Compute the delta between a timestamp taken in the past
* and now.
*
* This function is safe to use even if the timestamp is updated
* by an interrupt during execution.
*/
__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
/*
* Store the absolute time in an interrupt-safe fashion.
*
* This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
*/
__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
/*
* Call callout(arg) after delay has elapsed.
*
apps/drivers/drv_range_finder.h
+81
View File
@@ -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 */
apps/drivers/gps/gps.cpp
+2 -2
View File
@@ -67,7 +67,7 @@
#include "mtk.h"
#define TIMEOUT_5HZ 400
#define TIMEOUT_5HZ 500
#define RATE_MEASUREMENT_PERIOD 5000000
/* oddly, ERROR is not defined for c++ */
@@ -86,7 +86,7 @@ class GPS : public device::CDev
{
public:
GPS(const char* uart_path);
~GPS();
virtual ~GPS();
virtual int init();
apps/drivers/hil/hil.cpp
+1 -1
View File
@@ -91,7 +91,7 @@ public:
MODE_NONE
};
HIL();
~HIL();
virtual ~HIL();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
apps/drivers/hmc5883/hmc5883.cpp
+1 -1
View File
@@ -130,7 +130,7 @@ class HMC5883 : public device::I2C
{
public:
HMC5883(int bus);
~HMC5883();
virtual ~HMC5883();
virtual int init();
apps/drivers/l3gd20/l3gd20.cpp
+1 -1
View File
@@ -152,7 +152,7 @@ class L3GD20 : public device::SPI
{
public:
L3GD20(int bus, const char* path, spi_dev_e device);
~L3GD20();
virtual ~L3GD20();
virtual int init();
apps/drivers/led/led.cpp
+1 -1
View File
@@ -53,7 +53,7 @@ class LED : device::CDev
{
public:
LED();
~LED();
virtual ~LED();
virtual int init();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
apps/drivers/mb12xx/Makefile
+42
View File
@@ -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
apps/drivers/mb12xx/mb12xx.cpp
+840
View File
@@ -0,0 +1,840 @@
/****************************************************************************
*
* 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);
virtual ~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()
{
return measure();
}
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'");
}
apps/drivers/mpu6000/mpu6000.cpp
+1 -1
View File
@@ -151,7 +151,7 @@ class MPU6000 : public device::SPI
{
public:
MPU6000(int bus, spi_dev_e device);
~MPU6000();
virtual ~MPU6000();
virtual int init();
apps/drivers/ms5611/ms5611.cpp
+1 -1
View File
@@ -104,7 +104,7 @@ class MS5611 : public device::I2C
{
public:
MS5611(int bus);
~MS5611();
virtual ~MS5611();
virtual int init();
apps/drivers/px4fmu/fmu.cpp
+1 -1
View File
@@ -82,7 +82,7 @@ public:
MODE_NONE
};
PX4FMU();
~PX4FMU();
virtual ~PX4FMU();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
apps/drivers/px4io/px4io.cpp
+201 -33
View File
@@ -82,23 +82,28 @@
#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
{
public:
PX4IO();
~PX4IO();
virtual ~PX4IO();
virtual int init();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
virtual ssize_t write(file *filp, const char *buffer, size_t len);
void print_status();
private:
// XXX
unsigned _max_actuators;
unsigned _max_controls;
unsigned _max_rc_input;
unsigned _max_relays;
unsigned _max_transfer;
@@ -108,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 */
@@ -115,7 +122,7 @@ private:
uint16_t _alarms;
/* subscribed topics */
int _t_actuators; ///< actuator output topic
int _t_actuators; ///< actuator controls topic
int _t_armed; ///< system armed control topic
int _t_vstatus; ///< system / vehicle status
int _t_param; ///< parameter update topic
@@ -275,18 +282,21 @@ PX4IO *g_dev;
PX4IO::PX4IO() :
I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
_max_actuators(0),
_max_controls(0),
_max_rc_input(0),
_max_relays(0),
_max_transfer(16), /* sensible default */
_update_interval(0),
_status(0),
_alarms(0),
_task(-1),
_task_should_exit(false),
_mavlink_fd(-1),
_perf_update(perf_alloc(PC_ELAPSED, "px4io update")),
_status(0),
_alarms(0),
_t_actuators(-1),
_t_armed(-1),
_t_vstatus(-1),
_t_param(-1),
_to_input_rc(0),
_to_actuators_effective(0),
_to_outputs(0),
@@ -296,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;
}
@@ -339,6 +352,7 @@ PX4IO::init()
/* get some parameters */
_max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT);
_max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT);
_max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT);
_max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2;
_max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT);
@@ -348,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)
@@ -374,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).
@@ -459,10 +476,11 @@ PX4IO::init()
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0);
/* publish RC config to IO */
/* publish RC config to IO */
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;
}
@@ -484,6 +502,8 @@ PX4IO::init()
return -errno;
}
mavlink_log_info(_mavlink_fd, "[IO] init ok");
return OK;
}
@@ -634,11 +654,11 @@ PX4IO::io_set_control_state()
orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
ORB_ID(actuator_controls_1), _t_actuators, &controls);
for (unsigned i = 0; i < _max_actuators; i++)
for (unsigned i = 0; i < _max_controls; i++)
regs[i] = FLOAT_TO_REG(controls.control[i]);
/* copy values to registers in IO */
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators);
return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls);
}
int
@@ -689,21 +709,26 @@ PX4IO::io_set_rc_config()
for (unsigned i = 0; i < _max_rc_input; i++)
input_map[i] = -1;
/*
* NOTE: The indices for mapped channels are 1-based
* for compatibility reasons with existing
* autopilots / GCS'.
*/
param_get(param_find("RC_MAP_ROLL"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 0;
input_map[ichan - 1] = 0;
param_get(param_find("RC_MAP_PITCH"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 1;
input_map[ichan - 1] = 1;
param_get(param_find("RC_MAP_YAW"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 2;
input_map[ichan - 1] = 2;
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
if ((ichan >= 0) && (ichan < (int)_max_rc_input))
input_map[ichan] = 3;
input_map[ichan - 1] = 3;
ichan = 4;
for (unsigned i = 0; i < _max_rc_input; i++)
@@ -761,9 +786,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;
}
@@ -813,6 +845,8 @@ PX4IO::io_handle_alarms(uint16_t alarms)
/* set new alarms state */
_alarms = alarms;
return 0;
}
int
@@ -990,7 +1024,7 @@ PX4IO::io_publish_pwm_outputs()
/* convert from register format to float */
for (unsigned i = 0; i < _max_actuators; i++)
outputs.output[i] = REG_TO_FLOAT(ctl[i]);
outputs.output[i] = ctl[i];
outputs.noutputs = _max_actuators;
/* lazily advertise on first publication */
@@ -1142,18 +1176,134 @@ PX4IO::mixer_send(const char *buf, unsigned buflen)
} while (buflen > 0);
debug("mixer upload OK");
/* check for the mixer-OK flag */
if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK)
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;
debug("mixer rejected by IO");
} else {
debug("mixer rejected by IO");
mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail");
}
/* load must have failed for some reason */
return -EINVAL;
}
void
PX4IO::print_status()
{
/* basic configuration */
printf("protocol %u software %u bootloader %u buffer %uB\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER));
printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n",
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT),
io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT));
/* 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%s\n",
flags,
((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""),
((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"),
((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""),
((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""),
((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_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,
((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""),
((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : ""));
printf("vbatt %u ibatt %u\n",
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT),
io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT));
printf("actuators");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i));
printf("\n");
printf("servos");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));
printf("\n");
uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
printf("%d raw R/C inputs", raw_inputs);
for (unsigned i = 0; i < raw_inputs; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i));
printf("\n");
uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID);
printf("mapped R/C inputs 0x%04x", mapped_inputs);
for (unsigned i = 0; i < _max_rc_input; i++) {
if (mapped_inputs & (1 << i))
printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i)));
}
printf("\n");
uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT);
printf("ADC inputs");
for (unsigned i = 0; i < adc_inputs; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i));
printf("\n");
/* setup and state */
printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES));
uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
printf("arming 0x%04x%s%s%s%s\n",
arming,
((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""),
((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""));
printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS));
printf("vbatt scale %u ibatt scale %u ibatt bias %u\n",
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE),
io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS));
printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG));
printf("controls");
for (unsigned i = 0; i < _max_controls; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i));
printf("\n");
for (unsigned i = 0; i < _max_rc_input; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS);
printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n",
i,
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE),
io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT),
options,
((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""),
((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : ""));
}
printf("failsafe");
for (unsigned i = 0; i < _max_actuators; i++)
printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i));
printf("\n");
}
int
PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
{
@@ -1295,7 +1445,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
}
default:
/* not a recognised value */
/* not a recognized value */
ret = -ENOTTY;
}
@@ -1350,7 +1500,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) {
@@ -1422,7 +1572,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");
@@ -1433,7 +1583,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 {
@@ -1445,24 +1595,41 @@ 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")) {
if (g_dev != nullptr)
if (g_dev != nullptr) {
printf("[px4io] loaded\n");
else
g_dev->print_status();
} else {
printf("[px4io] not loaded\n");
}
exit(0);
}
@@ -1477,8 +1644,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);
apps/drivers/px4io/uploader.h
+1 -1
View File
@@ -46,7 +46,7 @@ class PX4IO_Uploader
{
public:
PX4IO_Uploader();
~PX4IO_Uploader();
virtual ~PX4IO_Uploader();
int upload(const char *filenames[]);
apps/drivers/stm32/drv_hrt.c
+30
View File
@@ -645,6 +645,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
ts->tv_nsec = abstime * 1000;
}
/*
* Compare a time value with the current time.
*/
hrt_abstime
hrt_elapsed_time(const volatile hrt_abstime *then)
{
irqstate_t flags = irqsave();
hrt_abstime delta = hrt_absolute_time() - *then;
irqrestore(flags);
return delta;
}
/*
* Store the absolute time in an interrupt-safe fashion
*/
hrt_abstime
hrt_store_absolute_time(volatile hrt_abstime *now)
{
irqstate_t flags = irqsave();
hrt_abstime ts = hrt_absolute_time();
irqrestore(flags);
return ts;
}
/*
* Initalise the high-resolution timing module.
*/
apps/drivers/stm32/drv_pwm_servo.c
+6 -2
View File
@@ -299,8 +299,12 @@ up_pwm_servo_arm(bool armed)
rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE;
} else {
/* on disarm, just stop auto-reload so we don't generate runts */
rCR1(i) &= ~GTIM_CR1_ARPE;
// XXX This leads to FMU PWM being still active
// but uncontrollable. Just disable the timer
// and risk a runt.
///* on disarm, just stop auto-reload so we don't generate runts */
//rCR1(i) &= ~GTIM_CR1_ARPE;
rCR1(i) = 0;
}
}
}
apps/examples/kalman_demo/KalmanNav.cpp
+2 -2
View File
@@ -247,8 +247,8 @@ void KalmanNav::update()
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
printf("nav: %4d Hz, miss #: %4d\n",
_navFrames / 10, _miss / 10);
//printf("nav: %4d Hz, miss #: %4d\n",
// _navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
apps/mavlink/mavlink_log.h
+12
View File
@@ -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];
apps/mavlink/orb_listener.c
+2 -18
View File
@@ -511,28 +511,12 @@ l_actuator_outputs(struct listener *l)
0);
} else {
/*
* Catch the case where no rudder is in use and throttle is not
* on output four
*/
float rudder, throttle;
if (act_outputs.noutputs < 4) {
rudder = 0.0f;
throttle = (act_outputs.output[2] - 900.0f) / 1200.0f;
} else {
rudder = (act_outputs.output[2] - 1500.0f) / 600.0f;
throttle = (act_outputs.output[3] - 900.0f) / 1200.0f;
}
mavlink_msg_hil_controls_send(chan,
hrt_absolute_time(),
(act_outputs.output[0] - 1500.0f) / 600.0f,
(act_outputs.output[1] - 1500.0f) / 600.0f,
rudder,
throttle,
(act_outputs.output[2] - 1500.0f) / 600.0f,
(act_outputs.output[3] - 900.0f) / 1200.0f,
(act_outputs.output[4] - 1500.0f) / 600.0f,
(act_outputs.output[5] - 1500.0f) / 600.0f,
(act_outputs.output[6] - 1500.0f) / 600.0f,
apps/mavlink_onboard/mavlink.c
+1 -1
View File
@@ -504,7 +504,7 @@ int mavlink_onboard_main(int argc, char *argv[])
mavlink_task = task_spawn("mavlink_onboard",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
6000 /* XXX probably excessive */,
2048,
mavlink_thread_main,
(const char**)argv);
exit(0);
apps/px4io/adc.c
+1 -1
View File
@@ -154,7 +154,7 @@ adc_measure(unsigned channel)
while (!(rSR & ADC_SR_EOC)) {
/* never spin forever - this will give a bogus result though */
if ((hrt_absolute_time() - now) > 1000) {
if (hrt_elapsed_time(&now) > 1000) {
debug("adc timeout");
break;
}
apps/px4io/controls.c
+191 -178
View File
@@ -39,13 +39,11 @@
#include <nuttx/config.h>
#include <stdbool.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <systemlib/ppm_decode.h>
//#define DEBUG
#include "px4io.h"
#define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */
@@ -54,21 +52,18 @@
static bool ppm_input(uint16_t *values, uint16_t *num_values);
void
controls_main(void)
{
struct pollfd fds[2];
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
static perf_counter_t c_gather_ppm;
void
controls_init(void)
{
/* DSM input */
fds[0].fd = dsm_init("/dev/ttyS0");
fds[0].events = POLLIN;
dsm_init("/dev/ttyS0");
/* S.bus input */
fds[1].fd = sbus_init("/dev/ttyS2");
fds[1].events = POLLIN;
ASSERT(fds[0].fd >= 0);
ASSERT(fds[1].fd >= 0);
sbus_init("/dev/ttyS2");
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
@@ -83,200 +78,218 @@ controls_main(void)
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
for (;;) {
/* run this loop at ~100Hz */
int result = poll(fds, 2, 10);
c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm");
c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus");
c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm");
}
ASSERT(result >= 0);
void
controls_tick() {
/*
* Gather R/C control inputs from supported sources.
*
* Note that if you're silly enough to connect more than
* one control input source, they're going to fight each
* other. Don't do that.
*/
/*
* Gather R/C control inputs from supported sources.
*
* Note that if you're silly enough to connect more than
* one control input source, they're going to fight each
* other. Don't do that.
*/
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
if (dsm_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
perf_begin(c_gather_dsm);
bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
if (dsm_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
perf_end(c_gather_dsm);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
if (sbus_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
perf_begin(c_gather_sbus);
bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count);
if (sbus_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
perf_end(c_gather_sbus);
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
if (ppm_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
/*
* XXX each S.bus frame will cause a PPM decoder interrupt
* storm (lots of edges). It might be sensible to actually
* disable the PPM decoder completely if we have S.bus signal.
*/
perf_begin(c_gather_ppm);
bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count);
if (ppm_updated)
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
perf_end(c_gather_ppm);
ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS);
/*
* In some cases we may have received a frame, but input has still
* been lost.
*/
bool rc_input_lost = false;
/*
* In some cases we may have received a frame, but input has still
* been lost.
*/
bool rc_input_lost = false;
/*
* If we received a new frame from any of the RC sources, process it.
*/
if (dsm_updated || sbus_updated || ppm_updated) {
/*
* If we received a new frame from any of the RC sources, process it.
*/
if (dsm_updated || sbus_updated || ppm_updated) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp = hrt_absolute_time();
/* update RC-received timestamp */
system_state.rc_channels_timestamp = hrt_absolute_time();
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
/* record a bitmask of channels assigned */
unsigned assigned_channels = 0;
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map raw inputs to mapped inputs */
/* XXX mapping should be atomic relative to protocol */
for (unsigned i = 0; i < r_raw_rc_count; i++) {
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
/* map the input channel */
uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {
uint16_t raw = r_raw_rc_values[i];
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 */
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];
/*
* 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 (-500..500) */
scaled -= 1500;
} 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;
ASSERT(scaled >= -15000);
ASSERT(scaled <= 15000);
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
ASSERT(mapped < MAX_CONTROL_CHANNELS);
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
/*
* If we got an update with zero channels, treat it as
* a loss of input.
*
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
if (assigned_channels == 0) {
rc_input_lost = true;
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
}
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
ASSERT(mapped < MAX_CONTROL_CHANNELS);
/*
* Export the valid channel bitmap
*/
r_rc_valid = assigned_channels;
r_rc_values[mapped] = SIGNED_TO_REG(scaled);
assigned_channels |= (1 << mapped);
}
}
/* set un-assigned controls to zero */
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
if (!(assigned_channels & (1 << i)))
r_rc_values[i] = 0;
}
/*
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
}
/*
* Handle losing RC input
*/
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
/* Mark the arrays as empty */
r_raw_rc_count = 0;
r_rc_valid = 0;
}
/*
* Check for manual override.
* If we got an update with zero channels, treat it as
* a loss of input.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
bool override = false;
/*
* Check mapped channel 5; 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;
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated)
mixer_tick();
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
if (assigned_channels == 0) {
rc_input_lost = true;
} else {
/* set RC OK flag and clear RC lost alarm */
r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST;
}
/*
* Export the valid channel bitmap
*/
r_rc_valid = assigned_channels;
}
/*
* If we haven't seen any new control data in 200ms, assume we
* have lost input.
*/
if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) {
rc_input_lost = true;
/* clear the input-kind flags here */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_RC_PPM |
PX4IO_P_STATUS_FLAGS_RC_DSM |
PX4IO_P_STATUS_FLAGS_RC_SBUS);
}
/*
* Handle losing RC input
*/
if (rc_input_lost) {
/* Clear the RC input status flag, clear manual override flag */
r_status_flags &= ~(
PX4IO_P_STATUS_FLAGS_OVERRIDE |
PX4IO_P_STATUS_FLAGS_RC_OK);
/* Set the RC_LOST alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
/* Mark the arrays as empty */
r_raw_rc_count = 0;
r_rc_valid = 0;
}
/*
* Check for manual override.
*
* The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
* must have R/C input.
* Override is enabled if either the hardcoded channel / value combination
* is selected, or the AP has requested it.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
bool override = false;
/*
* 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.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
override = true;
if (override) {
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
/* mix new RC input control values to servos */
if (dsm_updated || sbus_updated || ppm_updated)
mixer_tick();
} else {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
}
}
@@ -292,7 +305,7 @@ ppm_input(uint16_t *values, uint16_t *num_values)
* If we have received a new PPM frame within the last 200ms, accept it
* and then invalidate it.
*/
if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) {
if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
/* PPM data exists, copy it */
*num_values = ppm_decoded_channels;
apps/px4io/dsm.c
+3 -3
View File
@@ -249,18 +249,18 @@ dsm_guess_format(bool reset)
if ((votes11 == 1) && (votes10 == 0)) {
channel_shift = 11;
debug("DSM: detected 11-bit format");
debug("DSM: 11-bit format");
return;
}
if ((votes10 == 1) && (votes11 == 0)) {
channel_shift = 10;
debug("DSM: detected 10-bit format");
debug("DSM: 10-bit format");
return;
}
/* call ourselves to reset our state ... we have to try again */
debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11);
dsm_guess_format(true);
}
apps/px4io/mixer.cpp
+30 -76
View File
@@ -38,6 +38,7 @@
*/
#include <nuttx/config.h>
#include <syslog.h>
#include <sys/types.h>
#include <stdbool.h>
@@ -88,15 +89,20 @@ void
mixer_tick(void)
{
/* check that we are receiving fresh data from the FMU */
if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
debug("AP RX timeout");
lowsyslog("AP RX timeout");
}
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
/* XXX this is questionable - vehicle may not make sense for direct control */
r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
} else {
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
}
source = MIX_FAILSAFE;
@@ -104,9 +110,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 {
@@ -119,7 +127,8 @@ mixer_tick(void)
}
if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {
(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
@@ -157,71 +166,6 @@ mixer_tick(void)
r_page_servos[i] = 0;
}
#if 0
/* if everything is ok */
if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) {
/* we have recent control data from the FMU */
control_count = PX4IO_CONTROL_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
} else if (system_state.rc_channels > 0) {
/* when override is on or the fmu is not available, but RC is present */
control_count = system_state.rc_channels;
sched_lock();
/* remap roll, pitch, yaw and throttle from RC specific to internal ordering */
rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1];
rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1];
rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1];
rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1];
//rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1];
/* get the remaining channels, no remapping needed */
for (unsigned i = 4; i < system_state.rc_channels; i++) {
rc_channel_data[i] = system_state.rc_channel_data[i];
}
/* scale the control inputs */
rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) /
(float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000;
if (rc_channel_data[THROTTLE] > 2000) {
rc_channel_data[THROTTLE] = 2000;
}
if (rc_channel_data[THROTTLE] < 1000) {
rc_channel_data[THROTTLE] = 1000;
}
// lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n",
// (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]),
// (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE]));
control_values = &rc_channel_data[0];
sched_unlock();
} else {
/* we have no control input (no FMU, no RC) */
// XXX builtin failsafe would activate here
control_count = 0;
}
//lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]);
/* this is for multicopters, etc. where manual override does not make sense */
} else {
/* if the fmu is available whe are good */
if (system_state.mixer_fmu_available) {
control_count = PX4IO_CONTROL_CHANNELS;
control_values = &system_state.fmu_channel_data[0];
/* we better shut everything off */
} else {
control_count = 0;
}
}
#endif
/*
* Decide whether the servos should be armed right now.
*
@@ -231,9 +175,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 */
@@ -298,10 +244,15 @@ 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;
isr_debug(2, "mixer text %u", length);
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
return;
@@ -311,9 +262,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:
apps/px4io/protocol.h
+1
View File
@@ -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 */
apps/px4io/px4io.c
+65 -82
View File
@@ -64,18 +64,11 @@ struct sys_state_s system_state;
static struct hrt_call serial_dma_call;
/* global debug level for isr_debug() */
volatile uint8_t debug_level = 0;
/* store i2c reset count XXX this should be a register, together with other error counters */
volatile uint32_t i2c_loop_resets = 0;
struct hrt_call loop_overtime_call;
// this allows wakeup of the main task via a signal
static pid_t daemon_pid;
/*
a set of debug buffers to allow us to send debug information from ISRs
* a set of debug buffers to allow us to send debug information from ISRs
*/
static volatile uint32_t msg_counter;
@@ -83,19 +76,20 @@ static volatile uint32_t last_msg_counter;
static volatile uint8_t msg_next_out, msg_next_in;
/*
* WARNING too large buffers here consume the memory required
* WARNING: too large buffers here consume the memory required
* for mixer handling. Do not allocate more than 80 bytes for
* output.
*/
#define NUM_MSG 2
static char msg[NUM_MSG][50];
static char msg[NUM_MSG][40];
/*
add a debug message to be printed on the console
* add a debug message to be printed on the console
*/
void isr_debug(uint8_t level, const char *fmt, ...)
void
isr_debug(uint8_t level, const char *fmt, ...)
{
if (level > debug_level) {
if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) {
return;
}
va_list ap;
@@ -107,9 +101,10 @@ void isr_debug(uint8_t level, const char *fmt, ...)
}
/*
show all pending debug messages
* show all pending debug messages
*/
static void show_debug_messages(void)
static void
show_debug_messages(void)
{
if (msg_counter != last_msg_counter) {
uint32_t n = msg_counter - last_msg_counter;
@@ -122,36 +117,9 @@ static void show_debug_messages(void)
}
}
/*
catch I2C lockups
*/
static void loop_overtime(void *arg)
int
user_start(int argc, char *argv[])
{
debug("RESETTING\n");
i2c_loop_resets++;
i2c_dump();
i2c_reset();
hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL);
}
static void wakeup_handler(int signo, siginfo_t *info, void *ucontext)
{
// nothing to do - we just want poll() to return
}
/*
wakeup the main task using a signal
*/
void daemon_wakeup(void)
{
kill(daemon_pid, SIGUSR1);
}
int user_start(int argc, char *argv[])
{
daemon_pid = getpid();
/* run C++ ctors before we go any further */
up_cxxinitialize();
@@ -184,17 +152,8 @@ int user_start(int argc, char *argv[])
/* configure the first 8 PWM outputs (i.e. all of them) */
up_pwm_servo_init(0xff);
/* start the flight control signal handler */
task_create("FCon",
SCHED_PRIORITY_DEFAULT,
1024,
(main_t)controls_main,
NULL);
struct mallinfo minfo = mallinfo();
lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
debug("debug_level=%u\n", (unsigned)debug_level);
/* initialise the control inputs */
controls_init();
/* start the i2c handler */
i2c_init();
@@ -202,45 +161,69 @@ int user_start(int argc, char *argv[])
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/*
* setup a null handler for SIGUSR1 - we will use this for wakeup from poll()
*/
struct sigaction sa;
memset(&sa, 0, sizeof(sa));
sa.sa_sigaction = wakeup_handler;
sigfillset(&sa.sa_mask);
sigdelset(&sa.sa_mask, SIGUSR1);
if (sigaction(SIGUSR1, &sa, NULL) != OK) {
debug("Failed to setup SIGUSR1 handler\n");
}
/* add a performance counter for controls */
perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");
/* and one for measuring the loop rate */
perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");
struct mallinfo minfo = mallinfo();
lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);
#if 0
/* not enough memory, lock down */
if (minfo.mxordblk < 500) {
lowsyslog("ERR: not enough MEM");
bool phase = false;
if (phase) {
LED_AMBER(true);
LED_BLUE(false);
} else {
LED_AMBER(false);
LED_BLUE(true);
}
phase = !phase;
usleep(300000);
}
#endif
/*
* Run everything in a tight loop.
*/
/*
run the mixer at ~50Hz, using signals to run it early if
need be
*/
uint64_t last_debug_time = 0;
for (;;) {
/*
if we are not scheduled for 30ms then reset the I2C bus
*/
hrt_call_after(&loop_overtime_call, 30000, (hrt_callout)loop_overtime, NULL);
// we use usleep() instead of poll() as poll() is not
// interrupted by signals in nuttx, whereas usleep() is
usleep(20000);
/* track the rate at which the loop is running */
perf_count(loop_perf);
/* kick the mixer */
perf_begin(mixer_perf);
mixer_tick();
perf_end(mixer_perf);
/* kick the control inputs */
perf_begin(controls_perf);
controls_tick();
perf_end(controls_perf);
/* check for debug activity */
show_debug_messages();
if (hrt_absolute_time() - last_debug_time > 1000000) {
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u",
(unsigned)debug_level,
/* post debug state at ~1Hz */
if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {
struct mallinfo minfo = mallinfo();
isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u",
(unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
(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();
}
}
apps/px4io/px4io.h
+4 -8
View File
@@ -105,12 +105,12 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
*/
struct sys_state_s {
uint64_t rc_channels_timestamp;
volatile uint64_t rc_channels_timestamp;
/**
* Last FMU receive time, in microseconds since system boot
*/
uint64_t fmu_data_received_time;
volatile uint64_t fmu_data_received_time;
};
@@ -170,7 +170,8 @@ extern uint16_t adc_measure(unsigned channel);
*
* Input functions return true when they receive an update from the RC controller.
*/
extern void controls_main(void);
extern void controls_init(void);
extern void controls_tick(void);
extern int dsm_init(const char *device);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern int sbus_init(const char *device);
@@ -179,11 +180,6 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
/**
* Wake up mixer.
*/
void daemon_wakeup(void);
/* send a debug message to the console */
extern void isr_debug(uint8_t level, const char *fmt, ...);
apps/px4io/registers.c
+49 -26
View File
@@ -137,7 +137,8 @@ volatile uint16_t r_page_setup[] =
[PX4IO_P_SETUP_RELAYS] = 0,
[PX4IO_P_SETUP_VBATT_SCALE] = 10000,
[PX4IO_P_SETUP_IBATT_SCALE] = 0,
[PX4IO_P_SETUP_IBATT_BIAS] = 0
[PX4IO_P_SETUP_IBATT_BIAS] = 0,
[PX4IO_P_SETUP_SET_DEBUG] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
@@ -165,8 +166,8 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
*/
uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
/* valid options excluding ENABLE */
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE
/* valid options */
#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED)
/*
* PAGE 104 uses r_page_servos.
@@ -201,10 +202,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM;
// wake up daemon to trigger mixer
daemon_wakeup();
break;
/* handle raw PWM input */
@@ -224,8 +224,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
// wake up the main thread to trigger mixer
daemon_wakeup();
break;
/* handle setup for servo failsafe values */
@@ -355,8 +353,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
break;
case PX4IO_P_SETUP_SET_DEBUG:
debug_level = value;
isr_debug(0, "set debug %u\n", (unsigned)debug_level);
r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value;
isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
break;
default:
@@ -365,6 +363,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];
@@ -387,6 +392,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;
@@ -394,27 +400,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 */
apps/px4io/safety.c
+6 -1
View File
@@ -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;
}
apps/px4io/sbus.c
+4 -2
View File
@@ -53,7 +53,7 @@
#include "debug.h"
#define SBUS_FRAME_SIZE 25
#define SBUS_INPUT_CHANNELS 18
#define SBUS_INPUT_CHANNELS 16
static int sbus_fd = -1;
@@ -239,7 +239,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
}
/* decode switch channels if data fields are wide enough */
if (chancount > 17) {
if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) {
chancount = 18;
/* channel 17 (index 16) */
values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
apps/sdlog/sdlog.c
+1 -1
View File
@@ -683,7 +683,7 @@ int sdlog_thread_main(int argc, char *argv[])
.vbat = buf.batt.voltage_v,
.bat_current = buf.batt.current_a,
.bat_discharged = buf.batt.discharged_mah,
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]},
.adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]},
.local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z},
.gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt},
.attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw},
apps/sdlog/sdlog_ringbuffer.h
+2 -2
View File
@@ -56,7 +56,7 @@ struct sdlog_sysvector {
float vbat; /**< battery voltage in [volt] */
float bat_current; /**< battery discharge current */
float bat_discharged; /**< discharged energy in mAh */
float adc[3]; /**< remaining auxiliary ADC ports [volt] */
float adc[4]; /**< ADC ports [volt] */
float local_position[3]; /**< tangent plane mapping into x,y,z [m] */
int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */
float attitude[3]; /**< roll, pitch, yaw [rad] */
@@ -88,4 +88,4 @@ void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvec
int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem);
#endif
#endif
apps/sensors/sensor_params.c
+2
View File
@@ -64,6 +64,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f);
PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
apps/sensors/sensors.cpp
+52 -57
View File
@@ -187,6 +187,7 @@ private:
float mag_scale[3];
float accel_offset[3];
float accel_scale[3];
float airspeed_offset;
int rc_type;
@@ -233,6 +234,7 @@ private:
param_t accel_scale[3];
param_t mag_offset[3];
param_t mag_scale[3];
param_t airspeed_offset;
param_t rc_map_roll;
param_t rc_map_pitch;
@@ -475,6 +477,9 @@ Sensors::Sensors() :
_parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE");
_parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE");
/*Airspeed offset */
_parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF");
_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
/* fetch initial parameter values */
@@ -672,6 +677,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1]));
param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2]));
/* Airspeed offset */
param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset));
/* scaling of ADC ticks to battery voltage */
if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
warnx("Failed updating voltage scaling param");
@@ -975,12 +983,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* read all channels available */
int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc));
/* look for battery channel */
for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) {
if (ret >= (int)sizeof(buf_adc[0])) {
/* Save raw voltage values */
if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) {
raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f);
}
/* look for specific channels and process the raw voltage to measurement data */
if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* Voltage in volts */
float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling);
@@ -1010,8 +1022,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
/* calculate airspeed, raw is the difference from */
float voltage = buf_adc[i].am_data / 4096.0f;
float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor)
/**
* The voltage divider pulls the signal down, only act on
@@ -1019,24 +1030,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
*/
if (voltage > 0.4f) {
float pres_raw = fabsf(voltage - (3.3f / 2.0f));
float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f;
float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor
float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure,
_barometer.pressure, _barometer.temperature - 5.0f);
float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f,
_barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa
// XXX HACK - true temperature is much less than indicated temperature in baro,
// subtract 5 degrees in an attempt to account for the electrical upheating of the PCB
float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure,
_barometer.pressure, _barometer.temperature - 5.0f);
// XXX HACK
float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa);
//printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true);
_differential_pressure.timestamp = hrt_absolute_time();
_differential_pressure.static_pressure_mbar = _barometer.pressure;
_differential_pressure.differential_pressure_mbar = pres_mbar;
_differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f;
_differential_pressure.temperature_celcius = _barometer.temperature;
_differential_pressure.indicated_airspeed_m_s = airspeed_indicated;
_differential_pressure.true_airspeed_m_s = airspeed_true;
_differential_pressure.voltage = voltage;
/* announce the airspeed if needed, just publish else */
if (_airspeed_pub > 0) {
@@ -1058,36 +1069,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
void
Sensors::ppm_poll()
{
/* fake low-level driver, directly pulling from driver variables */
static orb_advert_t rc_input_pub = -1;
struct rc_input_values raw;
raw.timestamp = ppm_last_valid_decode;
/* we are accepting this message */
_ppm_last_valid = ppm_last_valid_decode;
/*
* relying on two decoded channels is very noise-prone,
* in particular if nothing is connected to the pins.
* requiring a minimum of four channels
*/
if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) {
for (unsigned i = 0; i < ppm_decoded_channels; i++) {
raw.values[i] = ppm_buffer[i];
}
raw.channel_count = ppm_decoded_channels;
/* publish to object request broker */
if (rc_input_pub <= 0) {
rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw);
} else {
orb_publish(ORB_ID(input_rc), rc_input_pub, &raw);
}
}
/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
bool rc_updated;
@@ -1133,31 +1114,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;
}
apps/systemcmds/mixer/mixer.c
+17 -1
View File
@@ -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))
apps/systemlib/airspeed.c
+42 -12
View File
@@ -40,14 +40,31 @@
*
*/
#include "math.h"
#include <stdio.h>
#include <math.h>
#include "conversions.h"
#include "airspeed.h"
float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature)
/**
* Calculate indicated airspeed.
*
* Note that the indicated airspeed is not the true airspeed because it
* lacks the air density compensation. Use the calc_true_airspeed functions to get
* the true airspeed.
*
* @param differential_pressure total_ pressure - static pressure
* @return indicated airspeed in m/s
*/
float calc_indicated_airspeed(float differential_pressure)
{
return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
if (differential_pressure > 0) {
return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
} else {
return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
}
}
/**
@@ -55,14 +72,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa
*
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @param speed current indicated airspeed
* @param speed_indicated current indicated airspeed
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @param temperature_celsius air temperature in degrees celcius
* @return true airspeed in m/s
*/
float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature)
float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius)
{
return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature));
return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius));
}
/**
@@ -70,12 +87,25 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo
*
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @param pressure_front pressure inside the pitot/prandl tube
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @param total_pressure pressure inside the pitot/prandtl tube
* @param static_pressure pressure at the side of the tube/airplane
* @param temperature_celsius air temperature in degrees celcius
* @return true airspeed in m/s
*/
float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature)
float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
{
return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature));
float density = get_air_density(static_pressure, temperature_celsius);
if (density < 0.0001f || !isfinite(density)) {
density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
printf("[airspeed] Invalid air density, using density at sea level\n");
}
float pressure_difference = total_pressure - static_pressure;
if(pressure_difference > 0) {
return sqrtf((2.0f*(pressure_difference)) / density);
} else
{
return -sqrtf((2.0f*fabs(pressure_difference)) / density);
}
}
apps/systemlib/airspeed.h
+34 -35
View File
@@ -48,43 +48,42 @@
__BEGIN_DECLS
/**
* Calculate indicated airspeed.
*
* Note that the indicated airspeed is not the true airspeed because it
* lacks the air density compensation. Use the calc_true_airspeed functions to get
* the true airspeed.
*
* @param pressure_front pressure inside the pitot/prandl tube
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return indicated airspeed in m/s
*/
__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature);
/**
* Calculate indicated airspeed.
*
* Note that the indicated airspeed is not the true airspeed because it
* lacks the air density compensation. Use the calc_true_airspeed functions to get
* the true airspeed.
*
* @param total_pressure pressure inside the pitot/prandtl tube
* @param static_pressure pressure at the side of the tube/airplane
* @return indicated airspeed in m/s
*/
__EXPORT float calc_indicated_airspeed(float differential_pressure);
/**
* Calculate true airspeed from indicated airspeed.
*
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @param speed current indicated airspeed
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return true airspeed in m/s
*/
__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature);
/**
* Calculate true airspeed from indicated airspeed.
*
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @param speed_indicated current indicated airspeed
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature_celsius air temperature in degrees celcius
* @return true airspeed in m/s
*/
__EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius);
/**
* Directly calculate true airspeed
*
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @param pressure_front pressure inside the pitot/prandl tube
* @param pressure_ambient pressure at the side of the tube/airplane
* @param temperature air temperature in degrees celcius
* @return true airspeed in m/s
*/
__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature);
/**
* Directly calculate true airspeed
*
* Note that the true airspeed is NOT the groundspeed, because of the effects of wind
*
* @param total_pressure pressure inside the pitot/prandtl tube
* @param static_pressure pressure at the side of the tube/airplane
* @param temperature_celsius air temperature in degrees celcius
* @return true airspeed in m/s
*/
__EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
__END_DECLS
apps/systemlib/conversions.c
+1 -1
View File
@@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9])
float get_air_density(float static_pressure, float temperature_celsius)
{
return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN));
return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
}
apps/systemlib/conversions.h
+4 -4
View File
@@ -44,10 +44,10 @@
#include <float.h>
#include <stdint.h>
#define CONSTANTS_ONE_G 9.80665f
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f
#define CONSTANTS_AIR_GAS_CONST 8.31432f
#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f
#define CONSTANTS_ONE_G 9.80665f // m/s^2
#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3
#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K)
#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C
__BEGIN_DECLS
apps/systemlib/mixer/mixer.cpp
+1
View File
@@ -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)
{
apps/systemlib/mixer/mixer.h
+1 -1
View File
@@ -160,7 +160,7 @@ public:
* @param control_cb Callback invoked when reading controls.
*/
Mixer(ControlCallback control_cb, uintptr_t cb_handle);
~Mixer() {};
virtual ~Mixer() {};
/**
* Perform the mixing function.
apps/systemlib/mixer/mixer_group.cpp
+1
View File
@@ -93,6 +93,7 @@ MixerGroup::reset()
mixer = _first;
_first = mixer->_next;
delete mixer;
mixer = nullptr;
}
}
apps/uORB/objects_common.cpp
+3
View File
@@ -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);
apps/uORB/topics/differential_pressure.h
+5 -4
View File
@@ -49,15 +49,16 @@
*/
/**
* Battery voltages and status
* Differential pressure and airspeed
*/
struct differential_pressure_s {
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
uint64_t timestamp; /**< microseconds since system boot, needed to integrate */
float static_pressure_mbar; /**< Static / environment pressure */
float differential_pressure_mbar; /**< Differential pressure reading */
float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */
float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */
float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */
};
/**
@@ -67,4 +68,4 @@ struct differential_pressure_s {
/* register this as object request broker structure */
ORB_DECLARE(differential_pressure);
#endif
#endif
apps/uORB/topics/subsystem_info.h
+2 -1
View File
@@ -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
};
/**
apps/uORB/topics/vehicle_status.h
+7 -7
View File
@@ -191,12 +191,16 @@ struct vehicle_status_s
bool condition_auto_mission_available;
bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */
bool condition_launch_position_valid; /**< indicates a valid launch position */
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool condition_local_position_valid;
bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
// bool condition_auto_flight_mode_ok; /**< conditions of auto flight mode apply plus a valid takeoff position lock has been aquired */
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */
bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */
bool flag_io_armed; /**< true is motors / actuators of IO are armed */
bool flag_io_armed; /**< true is motors / actuators of IO are armed */
bool flag_system_emergency;
bool flag_preflight_calibration;
@@ -208,10 +212,10 @@ struct vehicle_status_s
bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */
bool flag_control_position_enabled; /**< true if position is controlled */
// bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */
// bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */
// bool flag_preflight_accel_calibration;
// bool flag_preflight_airspeed_calibration;
bool rc_signal_found_once;
bool rc_signal_lost; /**< true if RC reception is terminally lost */
@@ -242,10 +246,6 @@ struct vehicle_status_s
uint16_t errors_count3;
uint16_t errors_count4;
// bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */
// bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */
bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */
};
/**
apps/uORB/uORB.cpp
+7
View File
@@ -429,6 +429,12 @@ ORBDevNode::appears_updated(SubscriberData *sd)
/* avoid racing between interrupt and non-interrupt context calls */
irqstate_t state = irqsave();
/* check if this topic has been published yet, if not bail out */
if (_data == nullptr) {
ret = false;
goto out;
}
/*
* If the subscriber's generation count matches the update generation
* count, there has been no update from their perspective; if they
@@ -485,6 +491,7 @@ ORBDevNode::appears_updated(SubscriberData *sd)
break;
}
out:
irqrestore(state);
/* consider it updated */
nuttx/configs/px4fmu/nsh/appconfig
+2 -1
View File
@@ -81,7 +81,7 @@ CONFIGURED_APPS += systemcmds/delay_test
# Tutorial code from
# https://pixhawk.ethz.ch/px4/dev/debug_values
CONFIGURED_APPS += examples/px4_mavlink_debug
# CONFIGURED_APPS += examples/px4_mavlink_debug
# Shared object broker; required by many parts of the system.
CONFIGURED_APPS += uORB
@@ -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
nuttx/configs/px4io/io/defconfig
+6 -6
View File
@@ -401,11 +401,11 @@ CONFIG_SCHED_ATEXIT=n
CONFIG_DISABLE_CLOCK=n
CONFIG_DISABLE_POSIX_TIMERS=y
CONFIG_DISABLE_PTHREAD=y
CONFIG_DISABLE_SIGNALS=n
CONFIG_DISABLE_SIGNALS=y
CONFIG_DISABLE_MQUEUE=y
CONFIG_DISABLE_MOUNTPOINT=y
CONFIG_DISABLE_ENVIRON=y
CONFIG_DISABLE_POLL=n
CONFIG_DISABLE_POLL=y
#
# Misc libc settings
@@ -475,12 +475,12 @@ CONFIG_ARCH_BZERO=n
# timer structures to minimize dynamic allocations. Set to
# zero for all dynamic allocations.
#
CONFIG_MAX_TASKS=8
CONFIG_MAX_TASKS=4
CONFIG_MAX_TASK_ARGS=4
CONFIG_NPTHREAD_KEYS=4
CONFIG_NPTHREAD_KEYS=2
CONFIG_NFILE_DESCRIPTORS=8
CONFIG_NFILE_STREAMS=0
CONFIG_NAME_MAX=32
CONFIG_NAME_MAX=12
CONFIG_STDIO_BUFFER_SIZE=32
CONFIG_STDIO_LINEBUFFER=n
CONFIG_NUNGET_CHARS=2
@@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n
CONFIG_CUSTOM_STACK=n
CONFIG_STACK_POINTER=
CONFIG_IDLETHREAD_STACKSIZE=1024
CONFIG_USERMAIN_STACKSIZE=1024
CONFIG_USERMAIN_STACKSIZE=1200
CONFIG_PTHREAD_STACK_MIN=512
CONFIG_PTHREAD_STACK_DEFAULT=1024
CONFIG_HEAP_BASE=