dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merge branch 'master' into export-build

This commit is contained in:
px4dev 2013-03-20 23:05:19 -07:00
parent f7b14b2e23 b7d65bf8fc
commit db91dffb23
98 changed files with 6673 additions and 2338 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

164
Debug/ARMv7M Normal file
View File

@ -0,0 +1,164 @@
#
# Assorted ARMv7M macros
#
echo Loading ARMv7M GDB macros. Use 'help armv7m' for more information.\n
define armv7m
echo Use 'help armv7m' for more information.\n
end
document armv7m
. Various macros for working with the ARMv7M family of processors.
.
. vecstate
. Print information about the current exception handling state.
.
. Use 'help <macro>' for more specific help.
end
define vecstate
set $icsr = *(unsigned *)0xe000ed04
set $vect = $icsr & 0x1ff
set $pend = ($icsr & 0x1ff000) >> 12
set $shcsr = *(unsigned *)0xe000ed24
set $cfsr = *(unsigned *)0xe000ed28
set $mmfsr = $cfsr & 0xff
set $bfsr = ($cfsr >> 8) & 0xff
set $ufsr = ($cfsr >> 16) & 0xffff
set $hfsr = *(unsigned *)0xe000ed2c
set $bfar = *(unsigned *)0xe000ed38
set $mmfar = *(unsigned *)0xe000ed34
if $vect < 15
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 $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
else
if $vect >= 15
printf "Handling vector %u\n", $vect
end
end
if ((unsigned)$lr & 0xf0000000) == 0xf0000000
if ($lr & 1)
printf "exception frame is on MSP\n"
#set $frame_ptr = (unsigned *)$msp
set $frame_ptr = (unsigned *)$sp
else
printf "exception frame is on PSP, backtrace may not be possible\n"
#set $frame_ptr = (unsigned *)$psp
set $frame_ptr = (unsigned *)$sp
end
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\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
set $sp = $fault_sp
set $saved_lr = $lr
set $lr = $frame_ptr[5]
set $saved_pc = $pc
set $pc = $frame_ptr[6]
bt
set $sp = $saved_sp
set $lr = $saved_lr
set $pc = $saved_pc
else
printf "(not currently in exception handler)\n"
end
end
document vecstate
. vecstate
. Print information about the current exception handling state.
end

12
Debug/NuttX
View File

@ -174,11 +174,15 @@ end
define showtaskstack
set $task = (struct _TCB *)$arg0
set $stack_free = 0
while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
set $stack_free = $stack_free + 1
if $task == &g_idletcb
printf "can't measure idle stack\n"
else
set $stack_free = 0
while ($stack_free < $task->adj_stack_size) && *(uint8_t *)($task->stack_alloc_ptr + $stack_free)
set $stack_free = $stack_free + 1
end
printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
end
printf" stack 0x%08x-0x%08x (%d) %d free\n", $task->stack_alloc_ptr, $task->adj_stack_ptr, $task->adj_stack_size, $stack_free
end
#

3
Debug/PX4
View File

@ -2,6 +2,7 @@
# Various PX4-specific macros
#
source Debug/NuttX
source Debug/ARMv7M
echo Loading PX4 GDB macros. Use 'help px4' for more information.\n
@ -21,7 +22,7 @@ end
define _perf_print
set $hdr = (struct perf_ctr_header *)$arg0
printf "%p\n", $hdr
#printf "%p\n", $hdr
printf "%s: ", $hdr->name
# PC_COUNT
if $hdr->type == 0

1
Firmware.sublime-project
View File

@ -7,6 +7,7 @@
[
"*.o",
"*.a",
"*.d",
".built",
".context",
".depend",

4
ROMFS/px4fmu_default/logging/logconv.m
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');

4
ROMFS/px4fmu_default/mixers/FMU_Q.mix
View File

@ -26,12 +26,12 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -5000 -8000 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -8000 -5000 0 -10000 10000
S: 0 1 8000 8000 0 -10000 10000
S: 0 1 -8000 -8000 0 -10000 10000
Output 2
--------

8
ROMFS/px4fmu_default/mixers/FMU_X5.mix
View File

@ -23,13 +23,13 @@ for the elevons.
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 3000 5000 0 -10000 10000
S: 0 1 5000 5000 0 -10000 10000
S: 0 0 -3000 -5000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 5000 3000 0 -10000 10000
S: 0 1 -5000 -5000 0 -10000 10000
S: 0 0 -5000 -3000 0 -10000 10000
S: 0 1 5000 5000 0 -10000 10000
Output 2
--------

2
Tools/px_mkfw.py
View File

@ -107,4 +107,4 @@ if args.image != None:
desc['image_size'] = len(bytes)
desc['image'] = base64.b64encode(zlib.compress(bytes,9))
print json.dumps(desc, indent=4)
print(json.dumps(desc, indent=4))

16
Tools/px_uploader.py
View File

@ -262,8 +262,8 @@ class uploader(object):
self.port.flush()
programmed = self.__recv(len(data))
if programmed != data:
print("got " + binascii.hexlify(programmed))
print("expect " + binascii.hexlify(data))
print(("got " + binascii.hexlify(programmed)))
print(("expect " + binascii.hexlify(data)))
return False
self.__getSync()
return True
@ -307,8 +307,8 @@ class uploader(object):
report_crc = self.__recv_int()
self.__getSync()
if report_crc != expect_crc:
print("Expected 0x%x" % expect_crc)
print("Got 0x%x" % report_crc)
print(("Expected 0x%x" % expect_crc))
print(("Got 0x%x" % report_crc))
raise RuntimeError("Program CRC failed")
# get basic data about the board
@ -319,7 +319,7 @@ class uploader(object):
# get the bootloader protocol ID first
self.bl_rev = self.__getInfo(uploader.INFO_BL_REV)
if (self.bl_rev < uploader.BL_REV_MIN) or (self.bl_rev > uploader.BL_REV_MAX):
print("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV)
print(("Unsupported bootloader protocol %d" % uploader.INFO_BL_REV))
raise RuntimeError("Bootloader protocol mismatch")
self.board_type = self.__getInfo(uploader.INFO_BOARD_ID)
@ -360,7 +360,7 @@ args = parser.parse_args()
# Load the firmware file
fw = firmware(args.firmware)
print("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision')))
print(("Loaded firmware for %x,%x, waiting for the bootloader..." % (fw.property('board_id'), fw.property('board_revision'))))
# Spin waiting for a device to show up
while True:
@ -393,7 +393,7 @@ while True:
try:
# identify the bootloader
up.identify()
print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
print(("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port)))
except:
# most probably a timeout talking to the port, no bootloader
@ -406,7 +406,7 @@ while True:
except RuntimeError as ex:
# print the error
print("ERROR: %s" % ex.args)
print(("ERROR: %s" % ex.args))
finally:
# always close the port

66
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -72,38 +72,6 @@
__EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]);
static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
static float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
static float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
static float x_aposteriori_k[12]; /**< states */
static float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
static float x_aposteriori[12];
static float P_aposteriori[144];
/* output euler angles */
static float euler[3] = {0.0f, 0.0f, 0.0f};
static float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int attitude_estimator_ekf_task; /**< Handle of deamon task / thread */
@ -153,7 +121,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
12000,
12400,
attitude_estimator_ekf_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL);
exit(0);
@ -193,6 +161,38 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
*/
int attitude_estimator_ekf_thread_main(int argc, char *argv[])
{
const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
float dt = 0.005f;
/* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */
float z_k[9] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 9.81f, 0.2f, -0.2f, 0.2f}; /**< Measurement vector */
float x_aposteriori_k[12]; /**< states */
float P_aposteriori_k[144] = {100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 100.f, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 100.0f, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 100.0f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.0f, 0, 0, 100.0f,
}; /**< init: diagonal matrix with big values */
float x_aposteriori[12];
float P_aposteriori[144];
/* output euler angles */
float euler[3] = {0.0f, 0.0f, 0.0f};
float Rot_matrix[9] = {1.f, 0, 0,
0, 1.f, 0,
0, 0, 1.f
}; /**< init: identity matrix */
// print text
printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
fflush(stdout);

138
apps/commander/commander.c
View File

@ -80,6 +80,7 @@
#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 <systemlib/param/param.h>
@ -242,11 +243,11 @@ static int led_off(int led)
}
enum AUDIO_PATTERN {
AUDIO_PATTERN_ERROR = 1,
AUDIO_PATTERN_NOTIFY_POSITIVE = 2,
AUDIO_PATTERN_NOTIFY_NEUTRAL = 3,
AUDIO_PATTERN_NOTIFY_NEGATIVE = 4,
AUDIO_PATTERN_TETRIS = 5
AUDIO_PATTERN_ERROR = 2,
AUDIO_PATTERN_NOTIFY_POSITIVE = 3,
AUDIO_PATTERN_NOTIFY_NEUTRAL = 4,
AUDIO_PATTERN_NOTIFY_NEGATIVE = 5,
AUDIO_PATTERN_NOTIFY_CHARGE = 6
};
int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state)
@ -785,6 +786,79 @@ 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");
/* set to accel calibration mode */
status->flag_preflight_airspeed_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
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)");
}
/* exit airspeed calibration mode */
status->flag_preflight_airspeed_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
close(sub_differential_pressure);
}
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
@ -980,6 +1054,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
handled = true;
}
/* airspeed calibration */
if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
/* transition to calibration state */
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
tune_confirm();
do_airspeed_calibration(status_pub, &current_status);
tune_confirm();
mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
handled = true;
}
/* none found */
if (!handled) {
//warnx("refusing unsupported calibration request\n");
@ -1265,6 +1361,8 @@ int commander_thread_main(int argc, char *argv[])
param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms);
param_t _param_sys_type = param_find("MAV_TYPE");
param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID");
/* welcome user */
warnx("I am in command now!\n");
@ -1379,6 +1477,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;
@ -1432,6 +1535,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) {
@ -1450,6 +1560,7 @@ int commander_thread_main(int argc, char *argv[])
/* parameters changed */
orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
/* update parameters */
if (!current_status.flag_system_armed) {
if (param_get(_param_sys_type, &(current_status.system_type)) != OK) {
@ -1465,6 +1576,11 @@ int commander_thread_main(int argc, char *argv[])
} else {
current_status.flag_external_manual_override_ok = true;
}
/* check and update system / component ID */
param_get(_param_system_id, &(current_status.system_id));
param_get(_param_component_id, &(current_status.component_id));
}
}
@ -1618,6 +1734,7 @@ 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.flag_global_position_valid;
bool local_pos_valid = current_status.flag_local_position_valid;
bool airspeed_valid = current_status.flag_airspeed_valid;
/* check for global or local position updates, set a timeout of 2s */
if (hrt_absolute_time() - last_global_position_time < 2000000) {
@ -1636,6 +1753,14 @@ int commander_thread_main(int argc, char *argv[])
current_status.flag_local_position_valid = false;
}
/* Check for valid airspeed/differential pressure measurements */
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
current_status.flag_airspeed_valid = true;
} else {
current_status.flag_airspeed_valid = false;
}
/*
* Consolidate global position and local position valid flags
* for vector flight mode.
@ -1651,7 +1776,8 @@ int commander_thread_main(int argc, char *argv[])
/* 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) {
local_pos_valid != current_status.flag_local_position_valid ||
airspeed_valid != current_status.flag_airspeed_valid) {
state_changed = true;
}

6
apps/controllib/block/BlockParam.cpp
View File

@ -46,7 +46,7 @@
namespace control
{
BlockParamBase::BlockParamBase(Block *parent, const char *name) :
BlockParamBase::BlockParamBase(Block *parent, const char *name, bool parent_prefix) :
_handle(PARAM_INVALID)
{
char fullname[blockNameLengthMax];
@ -61,8 +61,10 @@ BlockParamBase::BlockParamBase(Block *parent, const char *name) :
if (!strcmp(name, "")) {
strncpy(fullname, parentName, blockNameLengthMax);
} else {
} else if (parent_prefix) {
snprintf(fullname, blockNameLengthMax, "%s_%s", parentName, name);
} else {
strncpy(fullname, name, blockNameLengthMax);
}
parent->getParams().add(this);

11
apps/controllib/block/BlockParam.hpp
View File

@ -53,7 +53,12 @@ namespace control
class __EXPORT BlockParamBase : public ListNode<BlockParamBase *>
{
public:
BlockParamBase(Block *parent, const char *name);
/**
* Instantiate a block param base.
*
* @param parent_prefix Set to true to include the parent name in the parameter name
*/
BlockParamBase(Block *parent, const char *name, bool parent_prefix=true);
virtual ~BlockParamBase() {};
virtual void update() = 0;
const char *getName() { return param_name(_handle); }
@ -68,8 +73,8 @@ template<class T>
class __EXPORT BlockParam : public BlockParamBase
{
public:
BlockParam(Block *block, const char *name) :
BlockParamBase(block, name),
BlockParam(Block *block, const char *name, bool parent_prefix=true) :
BlockParamBase(block, name, parent_prefix),
_val() {
update();
}

14
apps/controllib/fixedwing.cpp
View File

@ -171,10 +171,10 @@ BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
_headingHold(this, ""),
_velocityHold(this, ""),
_altitudeHold(this, ""),
_trimAil(this, "TRIM_AIL"),
_trimElv(this, "TRIM_ELV"),
_trimRdr(this, "TRIM_RDR"),
_trimThr(this, "TRIM_THR")
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
_trimThr(this, "TRIM_THR", true) /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
{
}
@ -322,8 +322,8 @@ void BlockMultiModeBacksideAutopilot::update()
_att.roll, _att.pitch, _att.yaw,
_att.rollspeed, _att.pitchspeed, _att.yawspeed
);
_actuators.control[CH_AIL] = - _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = - _backsideAutopilot.getElevator();
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
@ -355,7 +355,7 @@ void BlockMultiModeBacksideAutopilot::update()
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = - _stabilization.getElevator();
_actuators.control[CH_ELV] = _stabilization.getElevator();
_actuators.control[CH_RDR] = _stabilization.getRudder();
_actuators.control[CH_THR] = _manual.throttle;
}

10
apps/drivers/blinkm/blinkm.cpp
View File

@ -127,7 +127,7 @@ class BlinkM : public device::I2C
{
public:
BlinkM(int bus, int blinkm);
~BlinkM();
virtual ~BlinkM();
virtual int init();
@ -135,7 +135,7 @@ public:
virtual int setMode(int mode);
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
static const char *script_names[];
static const char *const script_names[];
private:
enum ScriptID {
@ -219,7 +219,7 @@ namespace
}
/* list of script names, must match script ID numbers */
const char *BlinkM::script_names[] = {
const char *const BlinkM::script_names[] = {
"USER",
"RGB",
"WHITE_FLASH",
@ -499,7 +499,7 @@ BlinkM::led()
for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) {
if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break;
}
printf("<blinkm> cells found:%u\n", num_of_cells);
printf("<blinkm> cells found:%d\n", num_of_cells);
} else {
if(vehicle_status_raw.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
@ -827,7 +827,7 @@ BlinkM::get_firmware_version(uint8_t version[2])
{
const uint8_t msg = 'Z';
return transfer(&msg, sizeof(msg), version, sizeof(version));
return transfer(&msg, sizeof(msg), version, 2);
}
void blinkm_usage() {

2
apps/drivers/bma180/bma180.cpp
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();

14
apps/drivers/boards/px4fmu/px4fmu_spi.c
View File

@ -92,21 +92,21 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid,
case PX4_SPIDEV_GYRO:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
break;
case PX4_SPIDEV_ACCEL:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, !selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, selected);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
stm32_gpiowrite(GPIO_SPI_CS_MPU, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
break;
case PX4_SPIDEV_MPU:
/* Making sure the other peripherals are not selected */
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, selected);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, selected);
stm32_gpiowrite(GPIO_SPI_CS_ACCEL, 1);
stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1);
stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected);
break;
@ -125,7 +125,7 @@ __EXPORT uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, enum spi_dev_e devi
__EXPORT void stm32_spi3select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, bool selected)
{
/* there can only be one device on this bus, so always select it */
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, 0);
stm32_gpiowrite(GPIO_SPI_CS_SDCARD, !selected);
}
__EXPORT uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, enum spi_dev_e devid)

4
apps/drivers/boards/px4io/px4io_internal.h
View File

@ -74,8 +74,8 @@
#define GPIO_ACC_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN12)
#define GPIO_SERVO_OC_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTB|GPIO_PIN13)
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN13)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
#define GPIO_RELAY1_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN12)
#define GPIO_RELAY2_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTA|GPIO_PIN11)
/* Analog inputs ********************************************************************/

43
apps/drivers/device/i2c.cpp
View File

@ -120,7 +120,7 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
struct i2c_msg_s msgv[2];
unsigned msgs;
int ret;
unsigned tries = 0;
unsigned retry_count = 0;
do {
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
@ -154,16 +154,51 @@ I2C::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned re
I2C_SETFREQUENCY(_dev, _frequency);
ret = I2C_TRANSFER(_dev, &msgv[0], msgs);
/* success */
if (ret == OK)
break;
// reset the I2C bus to unwedge on error
up_i2creset(_dev);
/* if we have already retried once, or we are going to give up, then reset the bus */
if ((retry_count >= 1) || (retry_count >= _retries))
up_i2creset(_dev);
} while (tries++ < _retries);
} while (retry_count++ < _retries);
return ret;
}
int
I2C::transfer(i2c_msg_s *msgv, unsigned msgs)
{
int ret;
unsigned retry_count = 0;
/* force the device address into the message vector */
for (unsigned i = 0; i < msgs; i++)
msgv[i].addr = _address;
do {
/*
* I2C architecture means there is an unavoidable race here
* if there are any devices on the bus with a different frequency
* preference. Really, this is pointless.
*/
I2C_SETFREQUENCY(_dev, _frequency);
ret = I2C_TRANSFER(_dev, msgv, msgs);
/* success */
if (ret == OK)
break;
/* if we have already retried once, or we are going to give up, then reset the bus */
if ((retry_count >= 1) || (retry_count >= _retries))
up_i2creset(_dev);
} while (retry_count++ < _retries);
return ret;
}
} // namespace device

10
apps/drivers/device/i2c.h
View File

@ -100,6 +100,16 @@ protected:
int transfer(const uint8_t *send, unsigned send_len,
uint8_t *recv, unsigned recv_len);
/**
* Perform a multi-part I2C transaction to the device.
*
* @param msgv An I2C message vector.
* @param msgs The number of entries in the message vector.
* @return OK if the transfer was successful, -errno
* otherwise.
*/
int transfer(i2c_msg_s *msgv, unsigned msgs);
/**
* Change the bus address.
*

16
apps/drivers/drv_hrt.h
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.
*

46
apps/drivers/drv_pwm_output.h
View File

@ -36,7 +36,7 @@
*
* Servo values can be set with the PWM_SERVO_SET ioctl, by writing a
* pwm_output_values structure to the device, or by publishing to the
* output_pwm ObjDev.
* output_pwm ORB topic.
* Writing a value of 0 to a channel suppresses any output for that
* channel.
*/
@ -60,7 +60,7 @@ __BEGIN_DECLS
#define PWM_OUTPUT_DEVICE_PATH "/dev/pwm_output"
/**
* Maximum number of PWM output channels in the system.
* Maximum number of PWM output channels supported by the device.
*/
#define PWM_OUTPUT_MAX_CHANNELS 16
@ -77,22 +77,19 @@ typedef uint16_t servo_position_t;
* device.
*/
struct pwm_output_values {
/** desired servo update rate in Hz */
uint32_t update_rate;
/** desired pulse widths for each of the supported channels */
servo_position_t values[PWM_OUTPUT_MAX_CHANNELS];
};
/*
* ObjDev tag for PWM outputs.
* ORB tag for PWM outputs.
*/
ORB_DECLARE(output_pwm);
/*
* ioctl() definitions
*
* Note that ioctls and ObjDev updates should not be mixed, as the
* Note that ioctls and ORB updates should not be mixed, as the
* behaviour of the system in this case is not defined.
*/
#define _PWM_SERVO_BASE 0x2a00
@ -103,8 +100,14 @@ ORB_DECLARE(output_pwm);
/** disarm all servo outputs (stop generating pulses) */
#define PWM_SERVO_DISARM _IOC(_PWM_SERVO_BASE, 1)
/** set update rate in Hz */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** set alternate servo update rate */
#define PWM_SERVO_SET_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 2)
/** get the number of servos in *(unsigned *)arg */
#define PWM_SERVO_GET_COUNT _IOC(_PWM_SERVO_BASE, 3)
/** selects servo update rates, one bit per servo. 0 = default (50Hz), 1 = alternate */
#define PWM_SERVO_SELECT_UPDATE_RATE _IOC(_PWM_SERVO_BASE, 4)
/** set a single servo to a specific value */
#define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo)
@ -112,6 +115,10 @@ ORB_DECLARE(output_pwm);
/** get a single specific servo value */
#define PWM_SERVO_GET(_servo) _IOC(_PWM_SERVO_BASE, 0x40 + _servo)
/** get the _n'th rate group's channels; *(uint32_t *)arg returns a bitmap of channels
* whose update rates must be the same.
*/
#define PWM_SERVO_GET_RATEGROUP(_n) _IOC(_PWM_SERVO_BASE, 0x60 + _n)
/*
* Low-level PWM output interface.
@ -148,13 +155,32 @@ __EXPORT extern void up_pwm_servo_deinit(void);
__EXPORT extern void up_pwm_servo_arm(bool armed);
/**
* Set the servo update rate
* Set the servo update rate for all rate groups.
*
* @param rate The update rate in Hz to set.
* @return OK on success, -ERANGE if an unsupported update rate is set.
*/
__EXPORT extern int up_pwm_servo_set_rate(unsigned rate);
/**
* Get a bitmap of output channels assigned to a given rate group.
*
* @param group The rate group to query. Rate groups are assigned contiguously
* starting from zero.
* @return A bitmap of channels assigned to the rate group, or zero if
* the group number has no channels.
*/
__EXPORT extern uint32_t up_pwm_servo_get_rate_group(unsigned group);
/**
* Set the update rate for a given rate group.
*
* @param group The rate group whose update rate will be changed.
* @param rate The update rate in Hz.
* @return OK if the group was adjusted, -ERANGE if an unsupported update rate is set.
*/
__EXPORT extern int up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate);
/**
* Set the current output value for a channel.
*

81
apps/drivers/drv_range_finder.h Normal file
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 */

7
apps/drivers/drv_rc_input.h
View File

@ -100,4 +100,11 @@ struct rc_input_values {
*/
ORB_DECLARE(input_rc);
#define _RC_INPUT_BASE 0x2b00
/** Fetch R/C input values into (rc_input_values *)arg */
#define RC_INPUT_GET _IOC(_RC_INPUT_BASE, 0)
#endif /* _DRV_RC_INPUT_H */

4
apps/drivers/gps/gps.cpp
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();

88
apps/drivers/hil/hil.cpp
View File

@ -91,7 +91,7 @@ public:
MODE_NONE
};
HIL();
~HIL();
virtual ~HIL();
virtual int ioctl(file *filp, int cmd, unsigned long arg);
@ -158,6 +158,7 @@ HIL::HIL() :
CDev("hilservo", PWM_OUTPUT_DEVICE_PATH/*"/dev/hil" XXXL*/),
_mode(MODE_NONE),
_update_rate(50),
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
@ -511,9 +512,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate
g_hil->set_pwm_rate(arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
// HIL always outputs at the alternate (usually faster) rate
break;
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@ -549,6 +555,14 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_GET_RATEGROUP(0) ... PWM_SERVO_GET_RATEGROUP(PWM_OUTPUT_MAX_CHANNELS - 1): {
// no restrictions on output grouping
unsigned channel = cmd - PWM_SERVO_GET_RATEGROUP(0);
*(uint32_t *)arg = (1 << channel);
break;
}
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
@ -641,7 +655,7 @@ enum PortMode {
PortMode g_port_mode;
int
hil_new_mode(PortMode new_mode, int update_rate)
hil_new_mode(PortMode new_mode)
{
// uint32_t gpio_bits;
@ -699,8 +713,6 @@ hil_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */
g_hil->set_mode(servo_mode);
if ((servo_mode != HIL::MODE_NONE) && (update_rate != 0))
g_hil->set_pwm_rate(update_rate);
return OK;
}
@ -786,59 +798,34 @@ int
hil_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNDEFINED;
int pwm_update_rate_in_hz = 0;
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "fake"))
fake(argc - 1, argv + 1);
const char *verb = argv[1];
if (hil_start() != OK)
errx(1, "failed to start the FMU driver");
errx(1, "failed to start the HIL driver");
/*
* Mode switches.
*
* XXX use getopt?
*/
for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */
if (!strcmp(argv[i], "mode_pwm")) {
new_mode = PORT1_FULL_PWM;
// this was all cut-and-pasted from the FMU driver; it's junk
if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT1_FULL_PWM;
} else if (!strcmp(argv[i], "mode_pwm_serial")) {
new_mode = PORT1_PWM_AND_SERIAL;
} else if (!strcmp(verb, "mode_pwm_serial")) {
new_mode = PORT1_PWM_AND_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
new_mode = PORT1_PWM_AND_GPIO;
} else if (!strcmp(argv[i], "mode_port2_pwm8")) {
new_mode = PORT2_8PWM;
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT1_PWM_AND_GPIO;
} else if (!strcmp(argv[i], "mode_port2_pwm12")) {
new_mode = PORT2_8PWM;
} else if (!strcmp(verb, "mode_port2_pwm8")) {
new_mode = PORT2_8PWM;
} else if (!strcmp(argv[i], "mode_port2_pwm16")) {
new_mode = PORT2_8PWM;
}
} else if (!strcmp(verb, "mode_port2_pwm12")) {
new_mode = PORT2_8PWM;
/* look for the optional pwm update rate for the supported modes */
if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
// if (new_mode == PORT1_FULL_PWM || new_mode == PORT1_PWM_AND_GPIO) {
// XXX all modes have PWM settings
if (argc > i + 1) {
pwm_update_rate_in_hz = atoi(argv[i + 1]);
printf("pwm update rate: %d Hz\n", pwm_update_rate_in_hz);
} else {
fprintf(stderr, "missing argument for pwm update rate (-u)\n");
return 1;
}
// } else {
// fprintf(stderr, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio\n");
// }
}
}
} else if (!strcmp(verb, "mode_port2_pwm16")) {
new_mode = PORT2_8PWM;
}
/* was a new mode set? */
if (new_mode != PORT_MODE_UNDEFINED) {
@ -848,12 +835,17 @@ hil_main(int argc, char *argv[])
return OK;
/* switch modes */
return hil_new_mode(new_mode, pwm_update_rate_in_hz);
return hil_new_mode(new_mode);
}
/* test, etc. here */
if (!strcmp(verb, "test"))
test();
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
fprintf(stderr, "HIL: unrecognized command, try:\n");
fprintf(stderr, " mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
fprintf(stderr, " mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, mode_port2_pwm8, mode_port2_pwm12, mode_port2_pwm16\n");
return -EINVAL;
}

4
apps/drivers/hmc5883/hmc5883.cpp
View File

@ -130,7 +130,7 @@ class HMC5883 : public device::I2C
{
public:
HMC5883(int bus);
~HMC5883();
virtual ~HMC5883();
virtual int init();
@ -465,7 +465,7 @@ HMC5883::probe()
read_reg(ADDR_ID_C, data[2]))
debug("read_reg fail");
_retries = 1;
_retries = 2;
if ((data[0] != ID_A_WHO_AM_I) ||
(data[1] != ID_B_WHO_AM_I) ||

2
apps/drivers/l3gd20/l3gd20.cpp
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();

2
apps/drivers/led/led.cpp
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);

42
apps/drivers/mb12xx/Makefile Normal file
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

840
apps/drivers/mb12xx/mb12xx.cpp Normal file
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'");
}

2
apps/drivers/mpu6000/mpu6000.cpp
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();

70
apps/drivers/ms5611/ms5611.cpp
View File

@ -104,7 +104,7 @@ class MS5611 : public device::I2C
{
public:
MS5611(int bus);
~MS5611();
virtual ~MS5611();
virtual int init();
@ -144,6 +144,7 @@ private:
orb_advert_t _baro_topic;
perf_counter_t _sample_perf;
perf_counter_t _measure_perf;
perf_counter_t _comms_errors;
perf_counter_t _buffer_overflows;
@ -162,12 +163,12 @@ private:
* @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();
void start_cycle();
/**
* Stop the automatic measurement state machine.
*/
void stop();
void stop_cycle();
/**
* Perform a poll cycle; collect from the previous measurement
@ -274,6 +275,7 @@ MS5611::MS5611(int bus) :
_msl_pressure(101325),
_baro_topic(-1),
_sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")),
_measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")),
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
{
@ -287,7 +289,7 @@ MS5611::MS5611(int bus) :
MS5611::~MS5611()
{
/* make sure we are truly inactive */
stop();
stop_cycle();
/* free any existing reports */
if (_reports != nullptr)
@ -331,7 +333,11 @@ MS5611::probe()
if ((OK == probe_address(MS5611_ADDRESS_1)) ||
(OK == probe_address(MS5611_ADDRESS_2))) {
_retries = 1;
/*
* Disable retries; we may enable them selectively in some cases,
* but the device gets confused if we retry some of the commands.
*/
_retries = 0;
return OK;
}
@ -436,7 +442,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
stop_cycle();
_measure_ticks = 0;
return OK;
@ -458,7 +464,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if we need to start the poll state machine, do it */
if (want_start)
start();
start_cycle();
return OK;
}
@ -480,7 +486,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* if we need to start the poll state machine, do it */
if (want_start)
start();
start_cycle();
return OK;
}
@ -508,11 +514,11 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
return -ENOMEM;
/* reset the measurement state machine with the new buffer, free the old */
stop();
stop_cycle();
delete[] _reports;
_num_reports = arg;
_reports = buf;
start();
start_cycle();
return OK;
}
@ -545,7 +551,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
}
void
MS5611::start()
MS5611::start_cycle()
{
/* reset the report ring and state machine */
@ -558,7 +564,7 @@ MS5611::start()
}
void
MS5611::stop()
MS5611::stop_cycle()
{
work_cancel(HPWORK, &_work);
}
@ -574,15 +580,25 @@ MS5611::cycle_trampoline(void *arg)
void
MS5611::cycle()
{
int ret;
/* collection phase? */
if (_collect_phase) {
/* perform collection */
if (OK != collect()) {
log("collection error");
ret = collect();
if (ret != OK) {
if (ret == -6) {
/*
* The ms5611 seems to regularly fail to respond to
* its address; this happens often enough that we'd rather not
* spam the console with the message.
*/
} else {
//log("collection error %d", ret);
}
/* reset the collection state machine and try again */
start();
start_cycle();
return;
}
@ -609,8 +625,13 @@ MS5611::cycle()
}
/* measurement phase */
if (OK != measure())
log("measure error");
ret = measure();
if (ret != OK) {
//log("measure error %d", ret);
/* reset the collection state machine and try again */
start_cycle();
return;
}
/* next phase is collection */
_collect_phase = true;
@ -628,6 +649,8 @@ MS5611::measure()
{
int ret;
perf_begin(_measure_perf);
/*
* In phase zero, request temperature; in other phases, request pressure.
*/
@ -635,18 +658,25 @@ MS5611::measure()
/*
* Send the command to begin measuring.
*
* Disable retries on this command; we can't know whether failure
* means the device did or did not see the write.
*/
_retries = 0;
ret = transfer(&cmd_data, 1, nullptr, 0);
if (OK != ret)
perf_count(_comms_errors);
perf_end(_measure_perf);
return ret;
}
int
MS5611::collect()
{
int ret;
uint8_t cmd;
uint8_t data[3];
union {
@ -662,9 +692,11 @@ MS5611::collect()
/* this should be fairly close to the end of the conversion, so the best approximation of the time */
_reports[_next_report].timestamp = hrt_absolute_time();
if (OK != transfer(&cmd, 1, &data[0], 3)) {
ret = transfer(&cmd, 1, &data[0], 3);
if (ret != OK) {
perf_count(_comms_errors);
return -EIO;
perf_end(_sample_perf);
return ret;
}
/* fetch the raw value */

287
apps/drivers/px4fmu/fmu.cpp
View File

@ -64,12 +64,14 @@
#include <systemlib/err.h>
#include <systemlib/mixer/mixer.h>
#include <drivers/drv_mixer.h>
#include <drivers/drv_rc_input.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_effective.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
#include <systemlib/ppm_decode.h>
class PX4FMU : public device::CDev
{
@ -80,21 +82,26 @@ 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);
virtual int init();
int set_mode(Mode mode);
int set_pwm_rate(unsigned rate);
int set_pwm_alt_rate(unsigned rate);
int set_pwm_alt_channels(uint32_t channels);
private:
static const unsigned _max_actuators = 4;
Mode _mode;
int _update_rate;
int _current_update_rate;
unsigned _pwm_default_rate;
unsigned _pwm_alt_rate;
uint32_t _pwm_alt_rate_channels;
unsigned _current_update_rate;
int _task;
int _t_actuators;
int _t_armed;
@ -118,6 +125,7 @@ private:
uint8_t control_index,
float &input);
int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate);
int pwm_ioctl(file *filp, int cmd, unsigned long arg);
struct GPIOConfig {
@ -160,7 +168,10 @@ PX4FMU *g_fmu;
PX4FMU::PX4FMU() :
CDev("fmuservo", "/dev/px4fmu"),
_mode(MODE_NONE),
_update_rate(50),
_pwm_default_rate(50),
_pwm_alt_rate(50),
_pwm_alt_rate_channels(0),
_current_update_rate(0),
_task(-1),
_t_actuators(-1),
_t_armed(-1),
@ -260,27 +271,31 @@ PX4FMU::set_mode(Mode mode)
* are presented on the output pins.
*/
switch (mode) {
case MODE_2PWM:
debug("MODE_2PWM");
/* multi-port with flow control lines as PWM */
/* XXX magic numbers */
up_pwm_servo_init(0x3);
_update_rate = 50; /* default output rate */
break;
case MODE_2PWM: // multi-port with flow control lines as PWM
case MODE_4PWM: // multi-port as 4 PWM outs
debug("MODE_%dPWM", (mode == MODE_2PWM) ? 2 : 4);
/* default output rates */
_pwm_default_rate = 50;
_pwm_alt_rate = 50;
_pwm_alt_rate_channels = 0;
case MODE_4PWM:
debug("MODE_4PWM");
/* multi-port as 4 PWM outs */
/* XXX magic numbers */
up_pwm_servo_init(0xf);
_update_rate = 50; /* default output rate */
up_pwm_servo_init((mode == MODE_2PWM) ? 0x3 : 0xf);
set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate);
break;
case MODE_NONE:
debug("MODE_NONE");
/* disable servo outputs and set a very low update rate */
_pwm_default_rate = 10; /* artificially reduced output rate */
_pwm_alt_rate = 10;
_pwm_alt_rate_channels = 0;
/* disable servo outputs - no need to set rates */
up_pwm_servo_deinit();
_update_rate = 10;
break;
default:
@ -292,15 +307,63 @@ PX4FMU::set_mode(Mode mode)
}
int
PX4FMU::set_pwm_rate(unsigned rate)
PX4FMU::set_pwm_rate(uint32_t rate_map, unsigned default_rate, unsigned alt_rate)
{
if ((rate > 500) || (rate < 10))
return -EINVAL;
debug("set_pwm_rate %x %u %u", rate_map, default_rate, alt_rate);
for (unsigned pass = 0; pass < 2; pass++) {
for (unsigned group = 0; group < _max_actuators; group++) {
// get the channel mask for this rate group
uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0)
continue;
// all channels in the group must be either default or alt-rate
uint32_t alt = rate_map & mask;
if (pass == 0) {
// preflight
if ((alt != 0) && (alt != mask)) {
warn("rate group %u mask %x bad overlap %x", group, mask, alt);
// not a legal map, bail
return -EINVAL;
}
} else {
// set it - errors here are unexpected
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, _pwm_alt_rate) != OK) {
warn("rate group set alt failed");
return -EINVAL;
}
} else {
if (up_pwm_servo_set_rate_group_update(group, _pwm_default_rate) != OK) {
warn("rate group set default failed");
return -EINVAL;
}
}
}
}
}
_pwm_alt_rate_channels = rate_map;
_pwm_default_rate = default_rate;
_pwm_alt_rate = alt_rate;
_update_rate = rate;
return OK;
}
int
PX4FMU::set_pwm_alt_rate(unsigned rate)
{
return set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, rate);
}
int
PX4FMU::set_pwm_alt_channels(uint32_t channels)
{
return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate);
}
void
PX4FMU::task_main()
{
@ -338,33 +401,48 @@ PX4FMU::task_main()
unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4;
// rc input, published to ORB
struct rc_input_values rc_in;
orb_advert_t to_input_rc = 0;
memset(&rc_in, 0, sizeof(rc_in));
rc_in.input_source = RC_INPUT_SOURCE_PX4FMU_PPM;
log("starting");
/* loop until killed */
while (!_task_should_exit) {
/* handle update rate changes */
if (_current_update_rate != _update_rate) {
int update_rate_in_ms = int(1000 / _update_rate);
/*
* Adjust actuator topic update rate to keep up with
* the highest servo update rate configured.
*
* We always mix at max rate; some channels may update slower.
*/
unsigned max_rate = (_pwm_default_rate > _pwm_alt_rate) ? _pwm_default_rate : _pwm_alt_rate;
if (_current_update_rate != max_rate) {
_current_update_rate = max_rate;
int update_rate_in_ms = int(1000 / _current_update_rate);
/* reject faster than 500 Hz updates */
if (update_rate_in_ms < 2) {
update_rate_in_ms = 2;
_update_rate = 500;
}
/* reject slower than 50 Hz updates */
if (update_rate_in_ms > 20) {
update_rate_in_ms = 20;
_update_rate = 50;
/* reject slower than 10 Hz updates */
if (update_rate_in_ms > 100) {
update_rate_in_ms = 100;
}
debug("adjusted actuator update interval to %ums", update_rate_in_ms);
orb_set_interval(_t_actuators, update_rate_in_ms);
up_pwm_servo_set_rate(_update_rate);
_current_update_rate = _update_rate;
// set to current max rate, even if we are actually checking slower/faster
_current_update_rate = max_rate;
}
/* sleep waiting for data, but no more than a second */
int ret = ::poll(&fds[0], 2, 1000);
/* sleep waiting for data, stopping to check for PPM
* input at 100Hz */
int ret = ::poll(&fds[0], 2, 10);
/* this would be bad... */
if (ret < 0) {
@ -429,6 +507,26 @@ PX4FMU::task_main()
/* update PWM servo armed status if armed and not locked down */
up_pwm_servo_arm(aa.armed && !aa.lockdown);
}
// see if we have new PPM input data
if (ppm_last_valid_decode != rc_in.timestamp) {
// we have a new PPM frame. Publish it.
rc_in.channel_count = ppm_decoded_channels;
if (rc_in.channel_count > RC_INPUT_MAX_CHANNELS) {
rc_in.channel_count = RC_INPUT_MAX_CHANNELS;
}
for (uint8_t i=0; i<rc_in.channel_count; i++) {
rc_in.values[i] = ppm_buffer[i];
}
rc_in.timestamp = ppm_last_valid_decode;
/* lazily advertise on first publication */
if (to_input_rc == 0) {
to_input_rc = orb_advertise(ORB_ID(input_rc), &rc_in);
} else {
orb_publish(ORB_ID(input_rc), to_input_rc, &rc_in);
}
}
}
::close(_t_actuators);
@ -496,7 +594,6 @@ int
PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
{
int ret = OK;
int channel;
lock();
@ -510,7 +607,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_SET_UPDATE_RATE:
set_pwm_rate(arg);
ret = set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, arg);
break;
case PWM_SERVO_SELECT_UPDATE_RATE:
ret = set_pwm_rate(arg, _pwm_default_rate, _pwm_alt_rate);
break;
case PWM_SERVO_SET(2):
@ -524,9 +625,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
case PWM_SERVO_SET(0):
case PWM_SERVO_SET(1):
if (arg < 2100) {
channel = cmd - PWM_SERVO_SET(0);
up_pwm_servo_set(channel, arg);
up_pwm_servo_set(cmd - PWM_SERVO_SET(0), arg);
} else {
ret = -EINVAL;
}
@ -542,12 +641,18 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
channel = cmd - PWM_SERVO_GET(0);
*(servo_position_t *)arg = up_pwm_servo_get(channel);
break;
}
case PWM_SERVO_GET(1):
*(servo_position_t *)arg = up_pwm_servo_get(cmd - PWM_SERVO_GET(0));
break;
case PWM_SERVO_GET_RATEGROUP(0):
case PWM_SERVO_GET_RATEGROUP(1):
case PWM_SERVO_GET_RATEGROUP(2):
case PWM_SERVO_GET_RATEGROUP(3):
*(uint32_t *)arg = up_pwm_servo_get_rate_group(cmd - PWM_SERVO_GET_RATEGROUP(0));
break;
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
@ -621,6 +726,30 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
return ret;
}
/*
this implements PWM output via a write() method, for compatibility
with px4io
*/
ssize_t
PX4FMU::write(file *filp, const char *buffer, size_t len)
{
unsigned count = len / 2;
uint16_t values[4];
if (count > 4) {
// we only have 4 PWM outputs on the FMU
count = 4;
}
// allow for misaligned values
memcpy(values, buffer, count*2);
for (uint8_t i=0; i<count; i++) {
up_pwm_servo_set(i, values[i]);
}
return count * 2;
}
void
PX4FMU::gpio_reset(void)
{
@ -757,7 +886,7 @@ enum PortMode {
PortMode g_port_mode;
int
fmu_new_mode(PortMode new_mode, int update_rate)
fmu_new_mode(PortMode new_mode)
{
uint32_t gpio_bits;
PX4FMU::Mode servo_mode;
@ -809,9 +938,6 @@ fmu_new_mode(PortMode new_mode, int update_rate)
/* (re)set the PWM output mode */
g_fmu->set_mode(servo_mode);
if ((servo_mode != PX4FMU::MODE_NONE) && (update_rate != 0))
g_fmu->set_pwm_rate(update_rate);
return OK;
}
@ -907,57 +1033,31 @@ int
fmu_main(int argc, char *argv[])
{
PortMode new_mode = PORT_MODE_UNSET;
int pwm_update_rate_in_hz = 0;
if (!strcmp(argv[1], "test"))
test();
if (!strcmp(argv[1], "fake"))
fake(argc - 1, argv + 1);
const char *verb = argv[1];
if (fmu_start() != OK)
errx(1, "failed to start the FMU driver");
/*
* Mode switches.
*
* XXX use getopt?
*/
for (int i = 1; i < argc; i++) { /* argv[0] is "fmu" */
if (!strcmp(argv[i], "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(argv[i], "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm")) {
new_mode = PORT_FULL_PWM;
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
} else if (!strcmp(argv[i], "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
} else if (!strcmp(verb, "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
}
/* look for the optional pwm update rate for the supported modes */
if (strcmp(argv[i], "-u") == 0 || strcmp(argv[i], "--update-rate") == 0) {
if (new_mode == PORT_FULL_PWM || new_mode == PORT_PWM_AND_GPIO) {
if (argc > i + 1) {
pwm_update_rate_in_hz = atoi(argv[i + 1]);
} else {
errx(1, "missing argument for pwm update rate (-u)");
return 1;
}
} else {
errx(1, "pwm update rate currently only supported for mode_pwm, mode_pwm_gpio");
}
}
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT_PWM_AND_GPIO;
}
/* was a new mode set? */
@ -968,12 +1068,17 @@ fmu_main(int argc, char *argv[])
return OK;
/* switch modes */
return fmu_new_mode(new_mode, pwm_update_rate_in_hz);
int ret = fmu_new_mode(new_mode);
exit(ret == OK ? 0 : 1);
}
/* test, etc. here */
if (!strcmp(verb, "test"))
test();
if (!strcmp(verb, "fake"))
fake(argc - 1, argv + 1);
fprintf(stderr, "FMU: unrecognised command, try:\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm [-u pwm_update_rate_in_hz], mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
fprintf(stderr, " mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio\n");
exit(1);
}

1746
apps/drivers/px4io/px4io.cpp
View File

File diff suppressed because it is too large Load Diff

2
apps/drivers/px4io/uploader.h
View File

@ -46,7 +46,7 @@ class PX4IO_Uploader
{
public:
PX4IO_Uploader();
~PX4IO_Uploader();
virtual ~PX4IO_Uploader();
int upload(const char *filenames[]);

30
apps/drivers/stm32/drv_hrt.c
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.
*/

57
apps/drivers/stm32/drv_pwm_servo.c
View File

@ -68,10 +68,6 @@
#include <stm32_gpio.h>
#include <stm32_tim.h>
/* default rate (in Hz) of PWM updates */
static uint32_t pwm_update_rate = 50;
#define REG(_tmr, _reg) (*(volatile uint32_t *)(pwm_timers[_tmr].base + _reg))
#define rCR1(_tmr) REG(_tmr, STM32_GTIM_CR1_OFFSET)
@ -93,6 +89,10 @@ static uint32_t pwm_update_rate = 50;
#define rDCR(_tmr) REG(_tmr, STM32_GTIM_DCR_OFFSET)
#define rDMAR(_tmr) REG(_tmr, STM32_GTIM_DMAR_OFFSET)
static void pwm_timer_init(unsigned timer);
static void pwm_timer_set_rate(unsigned timer, unsigned rate);
static void pwm_channel_init(unsigned channel);
static void
pwm_timer_init(unsigned timer)
{
@ -113,11 +113,8 @@ pwm_timer_init(unsigned timer)
/* configure the timer to free-run at 1MHz */
rPSC(timer) = (pwm_timers[timer].clock_freq / 1000000) - 1;
/* and update at the desired rate */
rARR(timer) = (1000000 / pwm_update_rate) - 1;
/* generate an update event; reloads the counter and all registers */
rEGR(timer) = GTIM_EGR_UG;
/* default to updating at 50Hz */
pwm_timer_set_rate(timer, 50);
/* note that the timer is left disabled - arming is performed separately */
}
@ -272,19 +269,41 @@ up_pwm_servo_deinit(void)
}
int
up_pwm_servo_set_rate(unsigned rate)
up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
{
if ((rate < 50) || (rate > 400))
/* limit update rate to 1..10000Hz; somewhat arbitrary but safe */
if (rate < 1)
return -ERANGE;
if (rate > 10000)
return -ERANGE;
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++) {
if (pwm_timers[i].base != 0)
pwm_timer_set_rate(i, rate);
}
if ((group >= PWM_SERVO_MAX_TIMERS) || (pwm_timers[group].base == 0))
return ERROR;
pwm_timer_set_rate(group, rate);
return OK;
}
int
up_pwm_servo_set_rate(unsigned rate)
{
for (unsigned i = 0; i < PWM_SERVO_MAX_TIMERS; i++)
up_pwm_servo_set_rate_group_update(i, rate);
}
uint32_t
up_pwm_servo_get_rate_group(unsigned group)
{
unsigned channels = 0;
for (unsigned i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
if ((pwm_channels[i].gpio != 0) && (pwm_channels[i].timer_index == group))
channels |= (1 << i);
}
return channels;
}
void
up_pwm_servo_arm(bool armed)
{
@ -299,8 +318,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;
}
}
}

970
apps/drivers/stm32/tone_alarm/tone_alarm.cpp
View File

File diff suppressed because it is too large Load Diff

4
apps/examples/control_demo/params.c
View File

@ -56,8 +56,4 @@ PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity
// trim
PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)

4
apps/examples/kalman_demo/KalmanNav.cpp
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;
}

12
apps/mavlink/mavlink_log.h
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];

112
apps/mavlink/orb_listener.c
View File

@ -91,32 +91,32 @@ static uint64_t last_sensor_timestamp;
static void *uorb_receive_thread(void *arg);
struct listener {
void (*callback)(struct listener *l);
void (*callback)(const struct listener *l);
int *subp;
uintptr_t arg;
};
static void l_sensor_combined(struct listener *l);
static void l_vehicle_attitude(struct listener *l);
static void l_vehicle_gps_position(struct listener *l);
static void l_vehicle_status(struct listener *l);
static void l_rc_channels(struct listener *l);
static void l_input_rc(struct listener *l);
static void l_global_position(struct listener *l);
static void l_local_position(struct listener *l);
static void l_global_position_setpoint(struct listener *l);
static void l_local_position_setpoint(struct listener *l);
static void l_attitude_setpoint(struct listener *l);
static void l_actuator_outputs(struct listener *l);
static void l_actuator_armed(struct listener *l);
static void l_manual_control_setpoint(struct listener *l);
static void l_vehicle_attitude_controls(struct listener *l);
static void l_debug_key_value(struct listener *l);
static void l_optical_flow(struct listener *l);
static void l_vehicle_rates_setpoint(struct listener *l);
static void l_home(struct listener *l);
static void l_sensor_combined(const struct listener *l);
static void l_vehicle_attitude(const struct listener *l);
static void l_vehicle_gps_position(const struct listener *l);
static void l_vehicle_status(const struct listener *l);
static void l_rc_channels(const struct listener *l);
static void l_input_rc(const struct listener *l);
static void l_global_position(const struct listener *l);
static void l_local_position(const struct listener *l);
static void l_global_position_setpoint(const struct listener *l);
static void l_local_position_setpoint(const struct listener *l);
static void l_attitude_setpoint(const struct listener *l);
static void l_actuator_outputs(const struct listener *l);
static void l_actuator_armed(const struct listener *l);
static void l_manual_control_setpoint(const struct listener *l);
static void l_vehicle_attitude_controls(const struct listener *l);
static void l_debug_key_value(const struct listener *l);
static void l_optical_flow(const struct listener *l);
static void l_vehicle_rates_setpoint(const struct listener *l);
static void l_home(const struct listener *l);
struct listener listeners[] = {
static const struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
{l_vehicle_attitude, &mavlink_subs.att_sub, 0},
{l_vehicle_gps_position, &mavlink_subs.gps_sub, 0},
@ -144,7 +144,7 @@ struct listener listeners[] = {
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
void
l_sensor_combined(struct listener *l)
l_sensor_combined(const struct listener *l)
{
struct sensor_combined_s raw;
@ -199,7 +199,7 @@ l_sensor_combined(struct listener *l)
}
void
l_vehicle_attitude(struct listener *l)
l_vehicle_attitude(const struct listener *l)
{
struct vehicle_attitude_s att;
@ -222,7 +222,7 @@ l_vehicle_attitude(struct listener *l)
}
void
l_vehicle_gps_position(struct listener *l)
l_vehicle_gps_position(const struct listener *l)
{
struct vehicle_gps_position_s gps;
@ -256,7 +256,7 @@ l_vehicle_gps_position(struct listener *l)
}
void
l_vehicle_status(struct listener *l)
l_vehicle_status(const struct listener *l)
{
/* immediately communicate state changes back to user */
orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
@ -280,7 +280,7 @@ l_vehicle_status(struct listener *l)
}
void
l_rc_channels(struct listener *l)
l_rc_channels(const struct listener *l)
{
/* copy rc channels into local buffer */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
@ -288,7 +288,7 @@ l_rc_channels(struct listener *l)
}
void
l_input_rc(struct listener *l)
l_input_rc(const struct listener *l)
{
/* copy rc channels into local buffer */
orb_copy(ORB_ID(input_rc), mavlink_subs.input_rc_sub, &rc_raw);
@ -310,7 +310,7 @@ l_input_rc(struct listener *l)
}
void
l_global_position(struct listener *l)
l_global_position(const struct listener *l)
{
/* copy global position data into local buffer */
orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos);
@ -340,7 +340,7 @@ l_global_position(struct listener *l)
}
void
l_local_position(struct listener *l)
l_local_position(const struct listener *l)
{
/* copy local position data into local buffer */
orb_copy(ORB_ID(vehicle_local_position), mavlink_subs.local_pos_sub, &local_pos);
@ -357,7 +357,7 @@ l_local_position(struct listener *l)
}
void
l_global_position_setpoint(struct listener *l)
l_global_position_setpoint(const struct listener *l)
{
struct vehicle_global_position_setpoint_s global_sp;
@ -379,7 +379,7 @@ l_global_position_setpoint(struct listener *l)
}
void
l_local_position_setpoint(struct listener *l)
l_local_position_setpoint(const struct listener *l)
{
struct vehicle_local_position_setpoint_s local_sp;
@ -396,7 +396,7 @@ l_local_position_setpoint(struct listener *l)
}
void
l_attitude_setpoint(struct listener *l)
l_attitude_setpoint(const struct listener *l)
{
struct vehicle_attitude_setpoint_s att_sp;
@ -413,7 +413,7 @@ l_attitude_setpoint(struct listener *l)
}
void
l_vehicle_rates_setpoint(struct listener *l)
l_vehicle_rates_setpoint(const struct listener *l)
{
struct vehicle_rates_setpoint_s rates_sp;
@ -430,7 +430,7 @@ l_vehicle_rates_setpoint(struct listener *l)
}
void
l_actuator_outputs(struct listener *l)
l_actuator_outputs(const struct listener *l)
{
struct actuator_outputs_s act_outputs;
@ -511,32 +511,16 @@ 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[4] - 1500.0f) / 600.0f,
(act_outputs.output[5] - 1500.0f) / 600.0f,
(act_outputs.output[6] - 1500.0f) / 600.0f,
(act_outputs.output[7] - 1500.0f) / 600.0f,
(act_outputs.output[0] - 1500.0f) / 500.0f,
(act_outputs.output[1] - 1500.0f) / 500.0f,
(act_outputs.output[2] - 1500.0f) / 500.0f,
(act_outputs.output[3] - 1000.0f) / 1000.0f,
(act_outputs.output[4] - 1500.0f) / 500.0f,
(act_outputs.output[5] - 1500.0f) / 500.0f,
(act_outputs.output[6] - 1500.0f) / 500.0f,
(act_outputs.output[7] - 1500.0f) / 500.0f,
mavlink_mode,
0);
}
@ -545,13 +529,13 @@ l_actuator_outputs(struct listener *l)
}
void
l_actuator_armed(struct listener *l)
l_actuator_armed(const struct listener *l)
{
orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);
}
void
l_manual_control_setpoint(struct listener *l)
l_manual_control_setpoint(const struct listener *l)
{
struct manual_control_setpoint_s man_control;
@ -569,7 +553,7 @@ l_manual_control_setpoint(struct listener *l)
}
void
l_vehicle_attitude_controls(struct listener *l)
l_vehicle_attitude_controls(const struct listener *l)
{
struct actuator_controls_effective_s actuators;
@ -597,7 +581,7 @@ l_vehicle_attitude_controls(struct listener *l)
}
void
l_debug_key_value(struct listener *l)
l_debug_key_value(const struct listener *l)
{
struct debug_key_value_s debug;
@ -613,7 +597,7 @@ l_debug_key_value(struct listener *l)
}
void
l_optical_flow(struct listener *l)
l_optical_flow(const struct listener *l)
{
struct optical_flow_s flow;
@ -624,7 +608,7 @@ l_optical_flow(struct listener *l)
}
void
l_home(struct listener *l)
l_home(const struct listener *l)
{
struct home_position_s home;

2
apps/mavlink_onboard/mavlink.c
View File

@ -498,7 +498,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);

42
apps/namedapp/namedapp_list.h
View File

@ -1,42 +0,0 @@
/* List of application requirements, generated during make context. */
{ "math_demo", SCHED_PRIORITY_DEFAULT, 8192, math_demo_main },
{ "control_demo", SCHED_PRIORITY_DEFAULT, 2048, control_demo_main },
{ "kalman_demo", SCHED_PRIORITY_MAX - 30, 2048, kalman_demo_main },
{ "reboot", SCHED_PRIORITY_DEFAULT, 2048, reboot_main },
{ "perf", SCHED_PRIORITY_DEFAULT, 2048, perf_main },
{ "top", SCHED_PRIORITY_DEFAULT - 10, 3000, top_main },
{ "boardinfo", SCHED_PRIORITY_DEFAULT, 2048, boardinfo_main },
{ "mixer", SCHED_PRIORITY_DEFAULT, 4096, mixer_main },
{ "eeprom", SCHED_PRIORITY_DEFAULT, 4096, eeprom_main },
{ "param", SCHED_PRIORITY_DEFAULT, 4096, param_main },
{ "bl_update", SCHED_PRIORITY_DEFAULT, 4096, bl_update_main },
{ "preflight_check", SCHED_PRIORITY_DEFAULT, 2048, preflight_check_main },
{ "delay_test", SCHED_PRIORITY_DEFAULT, 2048, delay_test_main },
{ "uorb", SCHED_PRIORITY_DEFAULT, 4096, uorb_main },
{ "mavlink", SCHED_PRIORITY_DEFAULT, 2048, mavlink_main },
{ "mavlink_onboard", SCHED_PRIORITY_DEFAULT, 2048, mavlink_onboard_main },
{ "gps", SCHED_PRIORITY_DEFAULT, 2048, gps_main },
{ "commander", SCHED_PRIORITY_MAX - 30, 2048, commander_main },
{ "sdlog", SCHED_PRIORITY_MAX - 30, 2048, sdlog_main },
{ "sensors", SCHED_PRIORITY_MAX-5, 4096, sensors_main },
{ "ardrone_interface", SCHED_PRIORITY_MAX - 15, 2048, ardrone_interface_main },
{ "multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, multirotor_att_control_main },
{ "multirotor_pos_control", SCHED_PRIORITY_MAX - 25, 2048, multirotor_pos_control_main },
{ "fixedwing_att_control", SCHED_PRIORITY_MAX - 30, 2048, fixedwing_att_control_main },
{ "fixedwing_pos_control", SCHED_PRIORITY_MAX - 30, 2048, fixedwing_pos_control_main },
{ "position_estimator", SCHED_PRIORITY_DEFAULT, 4096, position_estimator_main },
{ "attitude_estimator_ekf", SCHED_PRIORITY_DEFAULT, 2048, attitude_estimator_ekf_main },
{ "ms5611", SCHED_PRIORITY_DEFAULT, 2048, ms5611_main },
{ "hmc5883", SCHED_PRIORITY_DEFAULT, 4096, hmc5883_main },
{ "mpu6000", SCHED_PRIORITY_DEFAULT, 4096, mpu6000_main },
{ "bma180", SCHED_PRIORITY_DEFAULT, 2048, bma180_main },
{ "l3gd20", SCHED_PRIORITY_DEFAULT, 2048, l3gd20_main },
{ "px4io", SCHED_PRIORITY_DEFAULT, 2048, px4io_main },
{ "blinkm", SCHED_PRIORITY_DEFAULT, 2048, blinkm_main },
{ "tone_alarm", SCHED_PRIORITY_DEFAULT, 2048, tone_alarm_main },
{ "adc", SCHED_PRIORITY_DEFAULT, 2048, adc_main },
{ "fmu", SCHED_PRIORITY_DEFAULT, 2048, fmu_main },
{ "hil", SCHED_PRIORITY_DEFAULT, 2048, hil_main },
{ "tests", SCHED_PRIORITY_DEFAULT, 12000, tests_main },
{ "sercon", SCHED_PRIORITY_DEFAULT, 2048, sercon_main },
{ "serdis", SCHED_PRIORITY_DEFAULT, 2048, serdis_main },

42
apps/namedapp/namedapp_proto.h
View File

@ -1,42 +0,0 @@
/* List of application entry points, generated during make context. */
EXTERN int math_demo_main(int argc, char *argv[]);
EXTERN int control_demo_main(int argc, char *argv[]);
EXTERN int kalman_demo_main(int argc, char *argv[]);
EXTERN int reboot_main(int argc, char *argv[]);
EXTERN int perf_main(int argc, char *argv[]);
EXTERN int top_main(int argc, char *argv[]);
EXTERN int boardinfo_main(int argc, char *argv[]);
EXTERN int mixer_main(int argc, char *argv[]);
EXTERN int eeprom_main(int argc, char *argv[]);
EXTERN int param_main(int argc, char *argv[]);
EXTERN int bl_update_main(int argc, char *argv[]);
EXTERN int preflight_check_main(int argc, char *argv[]);
EXTERN int delay_test_main(int argc, char *argv[]);
EXTERN int uorb_main(int argc, char *argv[]);
EXTERN int mavlink_main(int argc, char *argv[]);
EXTERN int mavlink_onboard_main(int argc, char *argv[]);
EXTERN int gps_main(int argc, char *argv[]);
EXTERN int commander_main(int argc, char *argv[]);
EXTERN int sdlog_main(int argc, char *argv[]);
EXTERN int sensors_main(int argc, char *argv[]);
EXTERN int ardrone_interface_main(int argc, char *argv[]);
EXTERN int multirotor_att_control_main(int argc, char *argv[]);
EXTERN int multirotor_pos_control_main(int argc, char *argv[]);
EXTERN int fixedwing_att_control_main(int argc, char *argv[]);
EXTERN int fixedwing_pos_control_main(int argc, char *argv[]);
EXTERN int position_estimator_main(int argc, char *argv[]);
EXTERN int attitude_estimator_ekf_main(int argc, char *argv[]);
EXTERN int ms5611_main(int argc, char *argv[]);
EXTERN int hmc5883_main(int argc, char *argv[]);
EXTERN int mpu6000_main(int argc, char *argv[]);
EXTERN int bma180_main(int argc, char *argv[]);
EXTERN int l3gd20_main(int argc, char *argv[]);
EXTERN int px4io_main(int argc, char *argv[]);
EXTERN int blinkm_main(int argc, char *argv[]);
EXTERN int tone_alarm_main(int argc, char *argv[]);
EXTERN int adc_main(int argc, char *argv[]);
EXTERN int fmu_main(int argc, char *argv[]);
EXTERN int hil_main(int argc, char *argv[]);
EXTERN int tests_main(int argc, char *argv[]);
EXTERN int sercon_main(int argc, char *argv[]);
EXTERN int serdis_main(int argc, char *argv[]);

4
apps/nshlib/Kconfig
View File

@ -55,6 +55,10 @@ config NSH_DISABLE_CP
bool "Disable cp"
default n
config NSH_DISABLE_CMP
bool "Disable cmp"
default n
config NSH_DISABLE_DD
bool "Disable dd"
default n

3
apps/nshlib/nsh.h
View File

@ -603,6 +603,9 @@ void nsh_usbtrace(void);
# ifndef CONFIG_NSH_DISABLE_CP
int cmd_cp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
# endif
# ifndef CONFIG_NSH_DISABLE_CMP
int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
# endif
# ifndef CONFIG_NSH_DISABLE_DD
int cmd_dd(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv);
# endif

81
apps/nshlib/nsh_fscmds.c
View File

@ -1232,3 +1232,84 @@ int cmd_sh(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
}
#endif
#endif
#if CONFIG_NFILE_DESCRIPTORS > 0
#ifndef CONFIG_NSH_DISABLE_CMP
int cmd_cmp(FAR struct nsh_vtbl_s *vtbl, int argc, char **argv)
{
char *path1 = NULL;
char *path2 = NULL;
int fd1 = -1, fd2 = -1;
int ret = ERROR;
unsigned total_read = 0;
/* Get the full path to the two files */
path1 = nsh_getfullpath(vtbl, argv[1]);
if (!path1)
{
goto errout;
}
path2 = nsh_getfullpath(vtbl, argv[2]);
if (!path2)
{
goto errout;
}
/* Open the files for reading */
fd1 = open(path1, O_RDONLY);
if (fd1 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO);
goto errout;
}
fd2 = open(path2, O_RDONLY);
if (fd2 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "open", NSH_ERRNO);
goto errout;
}
for (;;)
{
char buf1[128];
char buf2[128];
int nbytesread1 = read(fd1, buf1, sizeof(buf1));
int nbytesread2 = read(fd2, buf2, sizeof(buf2));
if (nbytesread1 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO);
goto errout;
}
if (nbytesread2 < 0)
{
nsh_output(vtbl, g_fmtcmdfailed, argv[0], "read", NSH_ERRNO);
goto errout;
}
total_read += nbytesread1>nbytesread2?nbytesread2:nbytesread1;
if (nbytesread1 != nbytesread2 || memcmp(buf1, buf2, nbytesread1) != 0)
{
nsh_output(vtbl, "files differ: byte %u\n", total_read);
goto errout;
}
if (nbytesread1 < sizeof(buf1)) break;
}
ret = OK;
errout:
if (fd1 != -1) close(fd1);
if (fd2 != -1) close(fd2);
return ret;
}
#endif
#endif

3
apps/nshlib/nsh_parse.c
View File

@ -175,6 +175,9 @@ static const struct cmdmap_s g_cmdmap[] =
# ifndef CONFIG_NSH_DISABLE_CP
{ "cp", cmd_cp, 3, 3, "<source-path> <dest-path>" },
# endif
# ifndef CONFIG_NSH_DISABLE_CMP
{ "cmp", cmd_cmp, 3, 3, "<path1> <path2>" },
# endif
#endif
#if defined (CONFIG_RTC) && !defined(CONFIG_DISABLE_CLOCK) && !defined(CONFIG_NSH_DISABLE_DATE)

2
apps/px4/tests/test_bson.c
View File

@ -51,7 +51,7 @@ static const int32_t sample_small_int = 123;
static const int64_t sample_big_int = (int64_t)INT_MAX + 123LL;
static const double sample_double = 2.5f;
static const char *sample_string = "this is a test";
static const uint8_t sample_data[256];
static const uint8_t sample_data[256] = {0};
//static const char *sample_filename = "/fs/microsd/bson.test";
static int

12
apps/px4io/Makefile
View File

@ -39,11 +39,19 @@
# Note that we pull a couple of specific files from the systemlib, since
# we can't support it all.
#
CSRCS = $(wildcard *.c) \
CSRCS = adc.c \
controls.c \
dsm.c \
i2c.c \
px4io.c \
registers.c \
safety.c \
sbus.c \
../systemlib/hx_stream.c \
../systemlib/perf_counter.c \
../systemlib/up_cxxinitialize.c
CXXSRCS = $(wildcard *.cpp)
CXXSRCS = mixer.cpp
INCLUDES = $(TOPDIR)/arch/arm/src/stm32 $(TOPDIR)/arch/arm/src/common

2
apps/px4io/adc.c
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;
}

329
apps/px4io/comms.c
View File

@ -1,329 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2012 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 comms.c
*
* FMU communication for the PX4IO module.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <poll.h>
#include <termios.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_pwm_output.h>
#include <systemlib/hx_stream.h>
#include <systemlib/perf_counter.h>
#define DEBUG
#include "px4io.h"
#define FMU_MIN_REPORT_INTERVAL 5000 /* 5ms */
#define FMU_MAX_REPORT_INTERVAL 100000 /* 100ms */
#define FMU_STATUS_INTERVAL 1000000 /* 100ms */
static int fmu_fd;
static hx_stream_t stream;
static struct px4io_report report;
static void comms_handle_frame(void *arg, const void *buffer, size_t length);
perf_counter_t comms_rx_errors;
static void
comms_init(void)
{
/* initialise the FMU interface */
fmu_fd = open("/dev/ttyS1", O_RDWR);
stream = hx_stream_init(fmu_fd, comms_handle_frame, NULL);
comms_rx_errors = perf_alloc(PC_COUNT, "rx_err");
hx_stream_set_counters(stream, 0, 0, comms_rx_errors);
/* default state in the report to FMU */
report.i2f_magic = I2F_MAGIC;
struct termios t;
/* 115200bps, no parity, one stop bit */
tcgetattr(fmu_fd, &t);
cfsetspeed(&t, 115200);
t.c_cflag &= ~(CSTOPB | PARENB);
tcsetattr(fmu_fd, TCSANOW, &t);
/* init the ADC */
adc_init();
}
void
comms_main(void)
{
comms_init();
struct pollfd fds;
fds.fd = fmu_fd;
fds.events = POLLIN;
debug("FMU: ready");
for (;;) {
/* wait for serial data, but no more than 10ms */
poll(&fds, 1, 10);
/*
* Pull bytes from FMU and feed them to the HX engine.
* Limit the number of bytes we actually process on any one iteration.
*/
if (fds.revents & POLLIN) {
char buf[32];
ssize_t count = read(fmu_fd, buf, sizeof(buf));
for (int i = 0; i < count; i++)
hx_stream_rx(stream, buf[i]);
}
/*
* Decide if it's time to send an update to the FMU.
*/
static hrt_abstime last_report_time;
hrt_abstime now, delta;
/* should we send a report to the FMU? */
now = hrt_absolute_time();
delta = now - last_report_time;
if ((delta > FMU_MIN_REPORT_INTERVAL) &&
(system_state.fmu_report_due || (delta > FMU_MAX_REPORT_INTERVAL))) {
system_state.fmu_report_due = false;
last_report_time = now;
/* populate the report */
for (unsigned i = 0; i < system_state.rc_channels; i++) {
report.rc_channel[i] = system_state.rc_channel_data[i];
}
report.channel_count = system_state.rc_channels;
report.armed = system_state.armed;
report.battery_mv = system_state.battery_mv;
report.adc_in = system_state.adc_in5;
report.overcurrent = system_state.overcurrent;
/* and send it */
hx_stream_send(stream, &report, sizeof(report));
}
/*
* Fetch ADC values, check overcurrent flags, etc.
*/
static hrt_abstime last_status_time;
if ((now - last_status_time) > FMU_STATUS_INTERVAL) {
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
*
* V counts
* 5 1001
* 6 1219
* 7 1436
* 8 1653
* 9 1870
* 10 2086
* 11 2303
* 12 2522
* 13 2738
* 14 2956
* 15 3172
* 16 3389
*
* slope = 0.0046067
* intercept = 0.3863
*
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
system_state.battery_mv = (4150 + (counts * 46)) / 10;
system_state.adc_in5 = adc_measure(ADC_IN5);
system_state.overcurrent =
(OVERCURRENT_SERVO ? (1 << 0) : 0) |
(OVERCURRENT_ACC ? (1 << 1) : 0);
last_status_time = now;
}
}
}
static void
comms_handle_config(const void *buffer, size_t length)
{
const struct px4io_config *cfg = (struct px4io_config *)buffer;
if (length != sizeof(*cfg))
return;
/* fetch the rc mappings */
for (unsigned i = 0; i < 4; i++) {
system_state.rc_map[i] = cfg->rc_map[i];
}
/* fetch the rc channel attributes */
for (unsigned i = 0; i < 4; i++) {
system_state.rc_min[i] = cfg->rc_min[i];
system_state.rc_trim[i] = cfg->rc_trim[i];
system_state.rc_max[i] = cfg->rc_max[i];
system_state.rc_rev[i] = cfg->rc_rev[i];
system_state.rc_dz[i] = cfg->rc_dz[i];
}
}
static void
comms_handle_command(const void *buffer, size_t length)
{
const struct px4io_command *cmd = (struct px4io_command *)buffer;
if (length != sizeof(*cmd))
return;
irqstate_t flags = irqsave();
/* fetch new PWM output values */
for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++)
system_state.fmu_channel_data[i] = cmd->output_control[i];
/* if the IO is armed and the FMU gets disarmed, the IO must also disarm */
if (system_state.arm_ok && !cmd->arm_ok)
system_state.armed = false;
system_state.arm_ok = cmd->arm_ok;
system_state.vector_flight_mode_ok = cmd->vector_flight_mode_ok;
system_state.manual_override_ok = cmd->manual_override_ok;
system_state.mixer_fmu_available = true;
system_state.fmu_data_received_time = hrt_absolute_time();
/* set PWM update rate if changed (after limiting) */
uint16_t new_servo_rate = cmd->servo_rate;
/* reject faster than 500 Hz updates */
if (new_servo_rate > 500) {
new_servo_rate = 500;
}
/* reject slower than 50 Hz updates */
if (new_servo_rate < 50) {
new_servo_rate = 50;
}
if (system_state.servo_rate != new_servo_rate) {
up_pwm_servo_set_rate(new_servo_rate);
system_state.servo_rate = new_servo_rate;
}
/*
* update servo values immediately.
* the updates are done in addition also
* in the mainloop, since this function will only
* update with a connected FMU.
*/
mixer_tick();
/* handle relay state changes here */
for (unsigned i = 0; i < PX4IO_RELAY_CHANNELS; i++) {
if (system_state.relays[i] != cmd->relay_state[i]) {
switch (i) {
case 0:
POWER_ACC1(cmd->relay_state[i]);
break;
case 1:
POWER_ACC2(cmd->relay_state[i]);
break;
case 2:
POWER_RELAY1(cmd->relay_state[i]);
break;
case 3:
POWER_RELAY2(cmd->relay_state[i]);
break;
}
}
system_state.relays[i] = cmd->relay_state[i];
}
irqrestore(flags);
}
static void
comms_handle_frame(void *arg, const void *buffer, size_t length)
{
const uint16_t *type = (const uint16_t *)buffer;
/* make sure it's what we are expecting */
if (length > 2) {
switch (*type) {
case F2I_MAGIC:
comms_handle_command(buffer, length);
break;
case F2I_CONFIG_MAGIC:
comms_handle_config(buffer, length);
break;
case F2I_MIXER_MAGIC:
mixer_handle_text(buffer, length);
break;
default:
break;
}
}
}

342
apps/px4io/controls.c
View File

@ -37,148 +37,296 @@
* R/C inputs and servo outputs.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <termios.h>
#include <string.h>
#include <poll.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include <systemlib/hx_stream.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 */
#define RC_CHANNEL_HIGH_THRESH 1700
#define RC_CHANNEL_LOW_THRESH 1300
#define RC_CHANNEL_HIGH_THRESH 5000
#define RC_CHANNEL_LOW_THRESH -5000
static void ppm_input(void);
static bool ppm_input(uint16_t *values, uint16_t *num_values);
static perf_counter_t c_gather_dsm;
static perf_counter_t c_gather_sbus;
static perf_counter_t c_gather_ppm;
void
controls_main(void)
controls_init(void)
{
struct pollfd fds[2];
/* 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;
sbus_init("/dev/ttyS2");
for (;;) {
/* run this loop at ~100Hz */
poll(fds, 2, 10);
/* default to a 1:1 input map, all enabled */
for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) {
unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = 0;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MIN] = 1000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_CENTER] = 1500;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_MAX] = 2000;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_DEADZONE] = 30;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_ASSIGNMENT] = i;
r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
}
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");
}
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.
*/
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);
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.
*/
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);
/*
* 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) {
/* update RC-received timestamp */
system_state.rc_channels_timestamp = hrt_absolute_time();
/* 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 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) {
uint16_t raw = r_raw_rc_values[i];
int16_t scaled;
/*
* 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];
/*
* 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]));
} 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]));
} else {
/* in the configured dead zone, output zero */
scaled = 0;
}
/* invert channel if requested */
if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
scaled = -scaled;
/* and update the scaled/mapped version */
unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
ASSERT(mapped < MAX_CONTROL_CHANNELS);
/* invert channel if pitch - pulling the lever down means pitching up by convention */
if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
scaled = -scaled;
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;
}
/*
* Gather R/C control inputs from supported sources.
* If we got an update with zero channels, treat it as
* a loss of input.
*
* 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.
* This might happen if a protocol-based receiver returns an update
* that contains no channels that we have mapped.
*/
bool locked = false;
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;
}
/*
* Store RC channel count to detect switch to RC loss sooner
* than just by timeout
* Export the valid channel bitmap
*/
unsigned rc_channels = system_state.rc_channels;
r_rc_valid = assigned_channels;
}
if (fds[0].revents & POLLIN)
locked |= dsm_input();
/*
* 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;
if (fds[1].revents & POLLIN)
locked |= sbus_input();
/* 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;
/*
* If we don't have lock from one of the serial receivers,
* look for PPM. It shares an input with S.bus, so there's
* a possibility it will mis-parse an S.bus frame.
* Check mapped channel 5 (can be any remote channel,
* depends on RC_MAP_OVER parameter);
* If the value is 'high' then the pilot has
* requested override.
*
* XXX 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 an alternate
* receiver lock.
*/
if (!locked)
ppm_input();
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
override = true;
/* check for manual override status */
if (system_state.rc_channel_data[4] > RC_CHANNEL_HIGH_THRESH) {
/* force manual input override */
system_state.mixer_manual_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 {
/* override not engaged, use FMU */
system_state.mixer_manual_override = false;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE;
}
/*
* If we haven't seen any new control data in 200ms, assume we
* have lost input and tell FMU.
*/
if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) {
if (system_state.rc_channels > 0) {
/*
* If the RC signal status (lost / present) has
* just changed, request an update immediately.
*/
system_state.fmu_report_due = true;
}
/* set the number of channels to zero - no inputs */
system_state.rc_channels = 0;
}
/*
* PWM output updates are performed in addition on each comm update.
* the updates here are required to ensure operation if FMU is not started
* or stopped responding.
*/
mixer_tick();
}
}
static void
ppm_input(void)
static bool
ppm_input(uint16_t *values, uint16_t *num_values)
{
/*
* Look for new PPM input.
*/
if (ppm_last_valid_decode != 0) {
bool result = false;
/* avoid racing with PPM updates */
irqstate_t state = irqsave();
/* avoid racing with PPM updates */
irqstate_t state = irqsave();
/*
* If we have received a new PPM frame within the last 200ms, accept it
* and then invalidate it.
*/
if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) {
/* PPM data exists, copy it */
system_state.rc_channels = ppm_decoded_channels;
*num_values = ppm_decoded_channels;
if (*num_values > MAX_CONTROL_CHANNELS)
*num_values = MAX_CONTROL_CHANNELS;
for (unsigned i = 0; i < ppm_decoded_channels; i++) {
system_state.rc_channel_data[i] = ppm_buffer[i];
}
for (unsigned i = 0; i < *num_values; i++)
values[i] = ppm_buffer[i];
/* copy the timestamp and clear it */
system_state.rc_channels_timestamp = ppm_last_valid_decode;
/* clear validity */
ppm_last_valid_decode = 0;
irqrestore(state);
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
/* good if we got any channels */
result = (*num_values > 0);
}
irqrestore(state);
return result;
}

50
apps/px4io/dsm.c
View File

@ -43,17 +43,13 @@
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <termios.h>
#include <systemlib/ppm_decode.h>
#include <drivers/drv_hrt.h>
#define DEBUG
#include "px4io.h"
#include "protocol.h"
#define DSM_FRAME_SIZE 16
#define DSM_FRAME_CHANNELS 7
@ -72,13 +68,13 @@ unsigned dsm_frame_drops;
static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
static void dsm_guess_format(bool reset);
static void dsm_decode(hrt_abstime now);
static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
int
dsm_init(const char *device)
{
if (dsm_fd < 0)
dsm_fd = open(device, O_RDONLY);
dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
if (dsm_fd >= 0) {
struct termios t;
@ -106,7 +102,7 @@ dsm_init(const char *device)
}
bool
dsm_input(void)
dsm_input(uint16_t *values, uint16_t *num_values)
{
ssize_t ret;
hrt_abstime now;
@ -143,7 +139,7 @@ dsm_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
return false;
last_rx_time = now;
@ -156,20 +152,14 @@ dsm_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < DSM_FRAME_SIZE)
goto out;
return false;
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
dsm_decode(now);
partial_frame_count = 0;
out:
/*
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
return dsm_decode(now, values, num_values);
}
static bool
@ -259,23 +249,23 @@ 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);
}
static void
dsm_decode(hrt_abstime frame_time)
static bool
dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
{
/*
@ -296,7 +286,7 @@ dsm_decode(hrt_abstime frame_time)
/* if we don't know the frame format, update the guessing state machine */
if (channel_shift == 0) {
dsm_guess_format(false);
return;
return false;
}
/*
@ -324,8 +314,8 @@ dsm_decode(hrt_abstime frame_time)
continue;
/* update the decoded channel count */
if (channel >= system_state.rc_channels)
system_state.rc_channels = channel + 1;
if (channel >= *num_values)
*num_values = channel + 1;
/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
if (channel_shift == 11)
@ -356,13 +346,11 @@ dsm_decode(hrt_abstime frame_time)
break;
}
system_state.rc_channel_data[channel] = value;
values[channel] = value;
}
/* and note that we have received data from the R/C controller */
/* XXX failsafe will cause problems here - need a strategy for detecting it */
system_state.rc_channels_timestamp = frame_time;
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
/*
* XXX Note that we may be in failsafe here; we need to work out how to detect that.
*/
return true;
}

340
apps/px4io/i2c.c Normal file
View File

@ -0,0 +1,340 @@
/****************************************************************************
*
* Copyright (C) 2012 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 i2c.c
*
* I2C communication for the PX4IO module.
*/
#include <stdint.h>
#include <nuttx/arch.h>
#include <arch/board/board.h>
#include <stm32_i2c.h>
#include <stm32_dma.h>
//#define DEBUG
#include "px4io.h"
/*
* I2C register definitions.
*/
#define I2C_BASE STM32_I2C1_BASE
#define REG(_reg) (*(volatile uint32_t *)(I2C_BASE + _reg))
#define rCR1 REG(STM32_I2C_CR1_OFFSET)
#define rCR2 REG(STM32_I2C_CR2_OFFSET)
#define rOAR1 REG(STM32_I2C_OAR1_OFFSET)
#define rOAR2 REG(STM32_I2C_OAR2_OFFSET)
#define rDR REG(STM32_I2C_DR_OFFSET)
#define rSR1 REG(STM32_I2C_SR1_OFFSET)
#define rSR2 REG(STM32_I2C_SR2_OFFSET)
#define rCCR REG(STM32_I2C_CCR_OFFSET)
#define rTRISE REG(STM32_I2C_TRISE_OFFSET)
static int i2c_interrupt(int irq, void *context);
static void i2c_rx_setup(void);
static void i2c_tx_setup(void);
static void i2c_rx_complete(void);
static void i2c_tx_complete(void);
static DMA_HANDLE rx_dma;
static DMA_HANDLE tx_dma;
static uint8_t rx_buf[68];
static unsigned rx_len;
static const uint8_t junk_buf[] = { 0xff, 0xff, 0xff, 0xff };
static const uint8_t *tx_buf = junk_buf;
static unsigned tx_len = sizeof(junk_buf);
unsigned tx_count;
static uint8_t selected_page;
static uint8_t selected_offset;
enum {
DIR_NONE = 0,
DIR_TX = 1,
DIR_RX = 2
} direction;
void
i2c_init(void)
{
debug("i2c init");
/* allocate DMA handles and initialise DMA */
rx_dma = stm32_dmachannel(DMACHAN_I2C1_RX);
i2c_rx_setup();
tx_dma = stm32_dmachannel(DMACHAN_I2C1_TX);
i2c_tx_setup();
/* enable the i2c block clock and reset it */
modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_I2C1EN);
modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_I2C1RST);
modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_I2C1RST, 0);
/* configure the i2c GPIOs */
stm32_configgpio(GPIO_I2C1_SCL);
stm32_configgpio(GPIO_I2C1_SDA);
/* soft-reset the block */
rCR1 |= I2C_CR1_SWRST;
rCR1 = 0;
/* set for DMA operation */
rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
/* set the frequency value in CR2 */
rCR2 &= ~I2C_CR2_FREQ_MASK;
rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
/* set divisor and risetime for fast mode */
uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
if (result < 1)
result = 1;
result = 3;
rCCR &= ~I2C_CCR_CCR_MASK;
rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
/* set our device address */
rOAR1 = 0x1a << 1;
/* enable event interrupts */
irq_attach(STM32_IRQ_I2C1EV, i2c_interrupt);
irq_attach(STM32_IRQ_I2C1ER, i2c_interrupt);
up_enable_irq(STM32_IRQ_I2C1EV);
up_enable_irq(STM32_IRQ_I2C1ER);
/* and enable the I2C port */
rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
#ifdef DEBUG
i2c_dump();
#endif
}
/*
reset the I2C bus
used to recover from lockups
*/
void i2c_reset(void)
{
rCR1 |= I2C_CR1_SWRST;
rCR1 = 0;
/* set for DMA operation */
rCR2 |= I2C_CR2_ITEVFEN |I2C_CR2_ITERREN | I2C_CR2_DMAEN;
/* set the frequency value in CR2 */
rCR2 &= ~I2C_CR2_FREQ_MASK;
rCR2 |= STM32_PCLK1_FREQUENCY / 1000000;
/* set divisor and risetime for fast mode */
uint16_t result = STM32_PCLK1_FREQUENCY / (400000 * 25);
if (result < 1)
result = 1;
result = 3;
rCCR &= ~I2C_CCR_CCR_MASK;
rCCR |= I2C_CCR_DUTY | I2C_CCR_FS | result;
rTRISE = (uint16_t)((((STM32_PCLK1_FREQUENCY / 1000000) * 300) / 1000) + 1);
/* set our device address */
rOAR1 = 0x1a << 1;
/* and enable the I2C port */
rCR1 |= I2C_CR1_ACK | I2C_CR1_PE;
}
static int
i2c_interrupt(int irq, FAR void *context)
{
uint16_t sr1 = rSR1;
if (sr1 & (I2C_SR1_STOPF | I2C_SR1_AF | I2C_SR1_ADDR)) {
if (sr1 & I2C_SR1_STOPF) {
/* write to CR1 to clear STOPF */
(void)rSR1; /* as recommended, re-read SR1 */
rCR1 |= I2C_CR1_PE;
}
/* DMA never stops, so we should do that now */
switch (direction) {
case DIR_TX:
i2c_tx_complete();
break;
case DIR_RX:
i2c_rx_complete();
break;
default:
/* not currently transferring - must be a new txn */
break;
}
direction = DIR_NONE;
}
if (sr1 & I2C_SR1_ADDR) {
/* clear ADDR to ack our selection and get direction */
(void)rSR1; /* as recommended, re-read SR1 */
uint16_t sr2 = rSR2;
if (sr2 & I2C_SR2_TRA) {
/* we are the transmitter */
direction = DIR_TX;
} else {
/* we are the receiver */
direction = DIR_RX;
}
}
/* clear any errors that might need it (this handles AF as well */
if (sr1 & I2C_SR1_ERRORMASK)
rSR1 = 0;
return 0;
}
static void
i2c_rx_setup(void)
{
/*
* Note that we configure DMA in circular mode; this means that a too-long
* transfer will overwrite the buffer, but that avoids us having to deal with
* bailing out of a transaction while the master is still babbling at us.
*/
rx_len = 0;
stm32_dmasetup(rx_dma, (uintptr_t)&rDR, (uintptr_t)&rx_buf[0], sizeof(rx_buf),
DMA_CCR_CIRC |
DMA_CCR_MINC |
DMA_CCR_PSIZE_32BITS |
DMA_CCR_MSIZE_8BITS |
DMA_CCR_PRIMED);
stm32_dmastart(rx_dma, NULL, NULL, false);
}
static void
i2c_rx_complete(void)
{
rx_len = sizeof(rx_buf) - stm32_dmaresidual(rx_dma);
stm32_dmastop(rx_dma);
if (rx_len >= 2) {
selected_page = rx_buf[0];
selected_offset = rx_buf[1];
/* work out how many registers are being written */
unsigned count = (rx_len - 2) / 2;
if (count > 0) {
registers_set(selected_page, selected_offset, (const uint16_t *)&rx_buf[2], count);
} else {
/* no registers written, must be an address cycle */
uint16_t *regs;
unsigned reg_count;
/* work out which registers are being addressed */
int ret = registers_get(selected_page, selected_offset, &regs, &reg_count);
if (ret == 0) {
tx_buf = (uint8_t *)regs;
tx_len = reg_count * 2;
} else {
tx_buf = junk_buf;
tx_len = sizeof(junk_buf);
}
/* disable interrupts while reconfiguring DMA for the selected registers */
irqstate_t flags = irqsave();
stm32_dmastop(tx_dma);
i2c_tx_setup();
irqrestore(flags);
}
}
/* prepare for the next transaction */
i2c_rx_setup();
}
static void
i2c_tx_setup(void)
{
/*
* Note that we configure DMA in circular mode; this means that a too-long
* transfer will copy the buffer more than once, but that avoids us having
* to deal with bailing out of a transaction while the master is still
* babbling at us.
*/
stm32_dmasetup(tx_dma, (uintptr_t)&rDR, (uintptr_t)&tx_buf[0], tx_len,
DMA_CCR_DIR |
DMA_CCR_CIRC |
DMA_CCR_MINC |
DMA_CCR_PSIZE_8BITS |
DMA_CCR_MSIZE_8BITS |
DMA_CCR_PRIMED);
stm32_dmastart(tx_dma, NULL, NULL, false);
}
static void
i2c_tx_complete(void)
{
tx_count = tx_len - stm32_dmaresidual(tx_dma);
stm32_dmastop(tx_dma);
/* for debug purposes, save the length of the last transmit as seen by the DMA */
/* leave tx_buf/tx_len alone, so that a retry will see the same data */
/* prepare for the next transaction */
i2c_tx_setup();
}
void
i2c_dump(void)
{
debug("CR1 0x%08x CR2 0x%08x", rCR1, rCR2);
debug("OAR1 0x%08x OAR2 0x%08x", rOAR1, rOAR2);
debug("CCR 0x%08x TRISE 0x%08x", rCCR, rTRISE);
debug("SR1 0x%08x SR2 0x%08x", rSR1, rSR2);
}

248
apps/px4io/mixer.cpp
View File

@ -38,22 +38,14 @@
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <syslog.h>
#include <sys/types.h>
#include <stdbool.h>
#include <string.h>
#include <assert.h>
#include <errno.h>
#include <unistd.h>
#include <fcntl.h>
#include <sched.h>
#include <debug.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/device.h>
#include <systemlib/mixer/mixer.h>
@ -75,13 +67,16 @@ extern "C" {
#define OVERRIDE 4
/* current servo arm/disarm state */
bool mixer_servos_armed = false;
static bool mixer_servos_armed = false;
/* selected control values and count for mixing */
static uint16_t *control_values;
static int control_count;
static uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
enum mixer_source {
MIX_NONE,
MIX_FMU,
MIX_OVERRIDE,
MIX_FAILSAFE
};
static mixer_source source;
static int mixer_callback(uintptr_t handle,
uint8_t control_group,
@ -93,87 +88,62 @@ static MixerGroup mixer_group(mixer_callback, 0);
void
mixer_tick(void)
{
bool should_arm;
/* 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) {
/* too many frames without FMU input, time to go to failsafe */
system_state.mixer_manual_override = true;
system_state.mixer_fmu_available = false;
}
if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) {
/*
* Decide which set of inputs we're using.
*/
/* this is for planes, where manual override makes sense */
if (system_state.manual_override_ok) {
/* 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;
/* too long without FMU input, time to go to failsafe */
if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) {
isr_debug(1, "AP RX timeout");
}
//lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]);
r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM);
r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST;
/* 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;
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST;
}
source = MIX_FAILSAFE;
/*
* Decide which set of controls we're using.
*/
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 or
not a valid mixer. */
source = MIX_NONE;
} else {
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) &&
(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
/* mix from FMU controls */
source = MIX_FMU;
}
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_MIXER_OK)) {
/* if allowed, mix from RC inputs directly */
source = MIX_OVERRIDE;
}
}
/*
* Run the mixers if we have any control data at all.
* Run the mixers.
*/
if (control_count > 0) {
if (source == MIX_FAILSAFE) {
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
r_page_servos[i] = r_page_servo_failsafe[i];
} else if (source != MIX_NONE) {
float outputs[IO_SERVO_COUNT];
unsigned mixed;
@ -181,31 +151,36 @@ mixer_tick(void)
mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT);
/* scale to PWM and update the servo outputs as required */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++) {
if (i < mixed) {
/* scale to servo output */
system_state.servos[i] = (outputs[i] * 500.0f) + 1500;
for (unsigned i = 0; i < mixed; i++) {
} else {
/* set to zero to inhibit PWM pulse output */
system_state.servos[i] = 0;
}
/* save actuator values for FMU readback */
r_page_actuators[i] = FLOAT_TO_REG(outputs[i]);
/* scale to servo output */
r_page_servos[i] = (outputs[i] * 500.0f) + 1500;
/*
* If we are armed, update the servo output.
*/
if (system_state.armed && system_state.arm_ok) {
up_pwm_servo_set(i, system_state.servos[i]);
}
}
for (unsigned i = mixed; i < IO_SERVO_COUNT; i++)
r_page_servos[i] = 0;
}
/*
* Decide whether the servos should be armed right now.
* A sufficient reason is armed state and either FMU or RC control inputs
*
* We must be armed, and we must have a PWM source; either raw from
* FMU or from the mixer.
*
* XXX correct behaviour for failsafe may require an additional case
* here.
*/
should_arm = system_state.armed && system_state.arm_ok && (control_count > 0);
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) &&
/* FMU is available or FMU is not available but override is an option */
((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ))
);
if (should_arm && !mixer_servos_armed) {
/* need to arm, but not armed */
@ -217,6 +192,12 @@ mixer_tick(void)
up_pwm_servo_arm(false);
mixer_servos_armed = false;
}
if (mixer_servos_armed) {
/* update the servo outputs. */
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
up_pwm_servo_set(i, r_page_servos[i]);
}
}
static int
@ -225,30 +206,54 @@ mixer_callback(uintptr_t handle,
uint8_t control_index,
float &control)
{
/* if the control index refers to an input that's not valid, we can't return it */
if (control_index >= control_count)
if (control_group != 0)
return -1;
/* scale from current PWM units (1000-2000) to mixer input values */
if (system_state.manual_override_ok && system_state.mixer_manual_override && control_index == 3) {
control = ((float)control_values[control_index] - 1000.0f) / 1000.0f;
} else {
control = ((float)control_values[control_index] - 1500.0f) / 500.0f;
switch (source) {
case MIX_FMU:
if (control_index < PX4IO_CONTROL_CHANNELS) {
control = REG_TO_FLOAT(r_page_controls[control_index]);
break;
}
return -1;
case MIX_OVERRIDE:
if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << control_index)) {
control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
break;
}
return -1;
case MIX_FAILSAFE:
case MIX_NONE:
/* XXX we could allow for configuration of per-output failsafe values */
return -1;
}
return 0;
}
static char mixer_text[256];
/*
* XXX error handling here should be more aggressive; currently it is
* possible to get STATUS_FLAGS_MIXER_OK set even though the mixer has
* not loaded faithfully.
*/
static char mixer_text[256]; /* large enough for one mixer */
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;
debug("mixer text %u", length);
isr_debug(2, "mix txt %u", length);
if (length < sizeof(px4io_mixdata))
return;
@ -257,23 +262,30 @@ mixer_handle_text(const void *buffer, size_t length)
switch (msg->action) {
case F2I_MIXER_ACTION_RESET:
debug("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;
/* FALLTHROUGH */
case F2I_MIXER_ACTION_APPEND:
debug("append %d", length);
isr_debug(2, "append %d", length);
/* check for overflow - this is really fatal */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text))
/* XXX could add just what will fit & try to parse, then repeat... */
if ((mixer_text_length + text_length + 1) > sizeof(mixer_text)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK;
return;
}
/* append mixer text and nul-terminate */
memcpy(&mixer_text[mixer_text_length], msg->text, text_length);
mixer_text_length += text_length;
mixer_text[mixer_text_length] = '\0';
debug("buflen %u", mixer_text_length);
isr_debug(2, "buflen %u", mixer_text_length);
/* process the text buffer, adding new mixers as their descriptions can be parsed */
unsigned resid = mixer_text_length;
@ -281,7 +293,11 @@ mixer_handle_text(const void *buffer, size_t length)
/* if anything was parsed */
if (resid != mixer_text_length) {
debug("used %u", mixer_text_length - resid);
/* ideally, this should test resid == 0 ? */
r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK;
isr_debug(2, "used %u", mixer_text_length - resid);
/* copy any leftover text to the base of the buffer for re-use */
if (resid > 0)

202
apps/px4io/protocol.h
View File

@ -31,67 +31,157 @@
*
****************************************************************************/
/**
* @file PX4FMU <-> PX4IO messaging protocol.
*
* This initial version of the protocol is very simple; each side transmits a
* complete update with each frame. This avoids the sending of many small
* messages and the corresponding complexity involved.
*/
#pragma once
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
#pragma pack(push, 1)
/**
* Periodic command from FMU to IO.
* @file protocol.h
*
* PX4IO I2C interface protocol.
*
* Communication is performed via writes to and reads from 16-bit virtual
* registers organised into pages of 255 registers each.
*
* The first two bytes of each write select a page and offset address
* respectively. Subsequent reads and writes increment the offset within
* the page.
*
* Most pages are readable or writable but not both.
*
* Note that some pages may permit offset values greater than 255, which
* can only be achieved by long writes. The offset does not wrap.
*
* Writes to unimplemented registers are ignored. Reads from unimplemented
* registers return undefined values.
*
* As convention, values that would be floating point in other parts of
* the PX4 system are expressed as signed integer values scaled by 10000,
* e.g. control values range from -10000..10000. Use the REG_TO_SIGNED and
* SIGNED_TO_REG macros to convert between register representation and
* the signed version, and REG_TO_FLOAT/FLOAT_TO_REG to convert to float.
*
* Note that the implementation of readable pages prefers registers within
* readable pages to be densely packed. Page numbers do not need to be
* packed.
*/
struct px4io_command {
uint16_t f2i_magic;
#define F2I_MAGIC 0x636d
uint16_t servo_rate;
uint16_t output_control[PX4IO_CONTROL_CHANNELS]; /**< PWM output rate in Hz */
bool relay_state[PX4IO_RELAY_CHANNELS]; /**< relay states as requested by FMU */
bool arm_ok; /**< FMU allows full arming */
bool vector_flight_mode_ok; /**< FMU aquired a valid position lock, ready for pos control */
bool manual_override_ok; /**< if true, IO performs a direct manual override */
};
#define PX4IO_CONTROL_CHANNELS 8
#define PX4IO_INPUT_CHANNELS 12
#define PX4IO_RELAY_CHANNELS 4
/**
* Periodic report from IO to FMU
*/
struct px4io_report {
uint16_t i2f_magic;
#define I2F_MAGIC 0x7570
/* Per C, this is safe for all 2's complement systems */
#define REG_TO_SIGNED(_reg) ((int16_t)(_reg))
#define SIGNED_TO_REG(_signed) ((uint16_t)(_signed))
uint16_t rc_channel[PX4IO_INPUT_CHANNELS];
bool armed;
uint8_t channel_count;
#define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f)
#define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
uint16_t battery_mv;
uint16_t adc_in;
uint8_t overcurrent;
};
/* static configuration page */
#define PX4IO_PAGE_CONFIG 0
#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */
#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */
#define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */
#define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */
#define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */
#define PX4IO_P_CONFIG_ACTUATOR_COUNT 5 /* hardcoded max actuator output count */
#define PX4IO_P_CONFIG_RC_INPUT_COUNT 6 /* hardcoded max R/C input count supported */
#define PX4IO_P_CONFIG_ADC_INPUT_COUNT 7 /* hardcoded max ADC inputs */
#define PX4IO_P_CONFIG_RELAY_COUNT 8 /* harcoded # of relay outputs */
/**
* As-needed config message from FMU to IO
*/
struct px4io_config {
uint16_t f2i_config_magic;
#define F2I_CONFIG_MAGIC 0x6366
/* dynamic status page */
#define PX4IO_PAGE_STATUS 1
#define PX4IO_P_STATUS_FREEMEM 0
#define PX4IO_P_STATUS_CPULOAD 1
uint8_t rc_map[4]; /**< channel ordering of roll, pitch, yaw, throttle */
uint16_t rc_min[4]; /**< min value for each channel */
uint16_t rc_trim[4]; /**< trim value for each channel */
uint16_t rc_max[4]; /**< max value for each channel */
int8_t rc_rev[4]; /**< rev value for each channel */
uint16_t rc_dz[4]; /**< dz value for each channel */
};
#define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */
#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */
#define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */
#define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_DSM (1 << 4) /* DSM input is valid */
#define PX4IO_P_STATUS_FLAGS_RC_SBUS (1 << 5) /* SBUS input is valid */
#define PX4IO_P_STATUS_FLAGS_FMU_OK (1 << 6) /* controls from FMU are valid */
#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 */
#define PX4IO_P_STATUS_ALARMS_TEMPERATURE (1 << 1) /* board temperature is high */
#define PX4IO_P_STATUS_ALARMS_SERVO_CURRENT (1 << 2) /* servo current limit was exceeded */
#define PX4IO_P_STATUS_ALARMS_ACC_CURRENT (1 << 3) /* accessory current limit was exceeded */
#define PX4IO_P_STATUS_ALARMS_FMU_LOST (1 << 4) /* timed out waiting for controls from FMU */
#define PX4IO_P_STATUS_ALARMS_RC_LOST (1 << 5) /* timed out waiting for RC input */
#define PX4IO_P_STATUS_ALARMS_PWM_ERROR (1 << 6) /* PWM configuration or output was bad */
#define PX4IO_P_STATUS_VBATT 4 /* battery voltage in mV */
#define PX4IO_P_STATUS_IBATT 5 /* battery current in cA */
/* array of post-mix actuator outputs, -10000..10000 */
#define PX4IO_PAGE_ACTUATORS 2 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* array of PWM servo output values, microseconds */
#define PX4IO_PAGE_SERVOS 3 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* array of raw RC input values, microseconds */
#define PX4IO_PAGE_RAW_RC_INPUT 4
#define PX4IO_P_RAW_RC_COUNT 0 /* number of valid channels */
#define PX4IO_P_RAW_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT channels from here */
/* array of scaled RC input values, -10000..10000 */
#define PX4IO_PAGE_RC_INPUT 5
#define PX4IO_P_RC_VALID 0 /* bitmask of valid controls */
#define PX4IO_P_RC_BASE 1 /* CONFIG_RC_INPUT_COUNT controls from here */
/* array of raw ADC values */
#define PX4IO_PAGE_RAW_ADC_INPUT 6 /* 0..CONFIG_ADC_INPUT_COUNT-1 */
/* PWM servo information */
#define PX4IO_PAGE_PWM_INFO 7
#define PX4IO_RATE_MAP_BASE 0 /* 0..CONFIG_ACTUATOR_COUNT bitmaps of PWM rate groups */
/* setup page */
#define PX4IO_PAGE_SETUP 100
#define PX4IO_P_SETUP_FEATURES 0
#define PX4IO_P_SETUP_ARMING 1 /* arming controls */
#define PX4IO_P_SETUP_ARMING_ARM_OK (1 << 0) /* OK to arm */
#define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */
#define PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK (1 << 3) /* OK to perform position / vector control (= position lock) */
#define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */
#define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */
#define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */
#define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */
#define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */
#define PX4IO_P_SETUP_IBATT_SCALE 7 /* battery current scaling factor (float) */
#define PX4IO_P_SETUP_IBATT_BIAS 8 /* battery current bias value */
#define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */
/* autopilot control values, -10000..10000 */
#define PX4IO_PAGE_CONTROLS 101 /* 0..CONFIG_CONTROL_COUNT */
/* raw text load to the mixer parser - ignores offset */
#define PX4IO_PAGE_MIXERLOAD 102
/* R/C channel config */
#define PX4IO_PAGE_RC_CONFIG 103 /* R/C input configuration */
#define PX4IO_P_RC_CONFIG_MIN 0 /* lowest input value */
#define PX4IO_P_RC_CONFIG_CENTER 1 /* center input value */
#define PX4IO_P_RC_CONFIG_MAX 2 /* highest input value */
#define PX4IO_P_RC_CONFIG_DEADZONE 3 /* band around center that is ignored */
#define PX4IO_P_RC_CONFIG_ASSIGNMENT 4 /* mapped input value */
#define PX4IO_P_RC_CONFIG_OPTIONS 5 /* channel options bitmask */
#define PX4IO_P_RC_CONFIG_OPTIONS_ENABLED (1 << 0)
#define PX4IO_P_RC_CONFIG_OPTIONS_REVERSE (1 << 1)
#define PX4IO_P_RC_CONFIG_STRIDE 6 /* spacing between channel config data */
/* PWM output - overrides mixer */
#define PX4IO_PAGE_DIRECT_PWM 104 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/* PWM failsafe values - zero disables the output */
#define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */
/**
* As-needed mixer data upload.
@ -99,18 +189,16 @@ struct px4io_config {
* This message adds text to the mixer text buffer; the text
* buffer is drained as the definitions are consumed.
*/
#pragma pack(push, 1)
struct px4io_mixdata {
uint16_t f2i_mixer_magic;
#define F2I_MIXER_MAGIC 0x6d74
uint8_t action;
#define F2I_MIXER_ACTION_RESET 0
#define F2I_MIXER_ACTION_APPEND 1
#define F2I_MIXER_ACTION_RESET 0
#define F2I_MIXER_ACTION_APPEND 1
char text[0]; /* actual text size may vary */
};
#pragma pack(pop)
/* maximum size is limited by the HX frame size */
#define F2I_MIXER_MAX_TEXT (HX_STREAM_MAX_FRAME - sizeof(struct px4io_mixdata))
#pragma pack(pop)

164
apps/px4io/px4io.c
View File

@ -37,20 +37,23 @@
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdio.h> // required for task_create
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <nuttx/clock.h>
#include <poll.h>
#include <signal.h>
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_hrt.h>
#include <systemlib/perf_counter.h>
#include <stm32_uart.h>
#define DEBUG
#include "px4io.h"
__EXPORT int user_start(int argc, char *argv[]);
@ -59,19 +62,79 @@ extern void up_cxxinitialize(void);
struct sys_state_s system_state;
int user_start(int argc, char *argv[])
static struct hrt_call serial_dma_call;
/* store i2c reset count XXX this should be a register, together with other error counters */
volatile uint32_t i2c_loop_resets = 0;
/*
* a set of debug buffers to allow us to send debug information from ISRs
*/
static volatile uint32_t msg_counter;
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
* for mixer handling. Do not allocate more than 80 bytes for
* output.
*/
#define NUM_MSG 2
static char msg[NUM_MSG][40];
/*
* add a debug message to be printed on the console
*/
void
isr_debug(uint8_t level, const char *fmt, ...)
{
if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) {
return;
}
va_list ap;
va_start(ap, fmt);
vsnprintf(msg[msg_next_in], sizeof(msg[0]), fmt, ap);
va_end(ap);
msg_next_in = (msg_next_in+1) % NUM_MSG;
msg_counter++;
}
/*
* show all pending debug messages
*/
static void
show_debug_messages(void)
{
if (msg_counter != last_msg_counter) {
uint32_t n = msg_counter - last_msg_counter;
if (n > NUM_MSG) n = NUM_MSG;
last_msg_counter = msg_counter;
while (n--) {
debug("%s", msg[msg_next_out]);
msg_next_out = (msg_next_out+1) % NUM_MSG;
}
}
}
int
user_start(int argc, char *argv[])
{
/* run C++ ctors before we go any further */
up_cxxinitialize();
/* reset all to zero */
memset(&system_state, 0, sizeof(system_state));
/* default to 50 Hz PWM outputs */
system_state.servo_rate = 50;
/* configure the high-resolution time/callout interface */
hrt_init();
/*
* Poll at 1ms intervals for received bytes that have not triggered
* a DMA event.
*/
hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
/* print some startup info */
lowsyslog("\nPX4IO: starting\n");
@ -89,17 +152,80 @@ 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);
/* initialise the control inputs */
controls_init();
/* start the i2c handler */
i2c_init();
/* add a performance counter for mixing */
perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
/* 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("free %u largest %u\n", minfo.mxordblk, minfo.fordblks);
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.
*/
uint64_t last_debug_time = 0;
for (;;) {
/* 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();
/* 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)minfo.mxordblk);
last_debug_time = hrt_absolute_time();
}
}
}
/* we're done here, go run the communications loop */
comms_main();
}

190
apps/px4io/px4io.h
View File

@ -63,130 +63,59 @@
# define debug(fmt, args...) do {} while(0)
#endif
/*
* Registers.
*/
extern uint16_t r_page_status[]; /* PX4IO_PAGE_STATUS */
extern uint16_t r_page_actuators[]; /* PX4IO_PAGE_ACTUATORS */
extern uint16_t r_page_servos[]; /* PX4IO_PAGE_SERVOS */
extern uint16_t r_page_raw_rc_input[]; /* PX4IO_PAGE_RAW_RC_INPUT */
extern uint16_t r_page_rc_input[]; /* PX4IO_PAGE_RC_INPUT */
extern uint16_t r_page_adc[]; /* PX4IO_PAGE_RAW_ADC_INPUT */
extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */
extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */
extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */
extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */
/*
* Register aliases.
*
* Handy aliases for registers that are widely used.
*/
#define r_status_flags r_page_status[PX4IO_P_STATUS_FLAGS]
#define r_status_alarms r_page_status[PX4IO_P_STATUS_ALARMS]
#define r_raw_rc_count r_page_raw_rc_input[PX4IO_P_RAW_RC_COUNT]
#define r_raw_rc_values (&r_page_raw_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_rc_valid r_page_rc_input[PX4IO_P_RC_VALID]
#define r_rc_values (&r_page_rc_input[PX4IO_P_RAW_RC_BASE])
#define r_setup_features r_page_setup[PX4IO_P_SETUP_FEATURES]
#define r_setup_arming r_page_setup[PX4IO_P_SETUP_ARMING]
#define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES]
#define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE]
#define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE]
#define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS]
#define r_control_values (&r_page_controls[0])
/*
* System state structure.
*/
struct sys_state_s {
bool armed; /* IO armed */
bool arm_ok; /* FMU says OK to arm */
uint16_t servo_rate; /* output rate of servos in Hz */
/**
* Remote control input(s) channel mappings
*/
uint8_t rc_map[4];
/**
* Remote control channel attributes
*/
uint16_t rc_min[4];
uint16_t rc_trim[4];
uint16_t rc_max[4];
int16_t rc_rev[4];
uint16_t rc_dz[4];
/**
* Data from the remote control input(s)
*/
unsigned rc_channels;
uint16_t rc_channel_data[PX4IO_INPUT_CHANNELS];
uint64_t rc_channels_timestamp;
/**
* Control signals from FMU.
*/
uint16_t fmu_channel_data[PX4IO_CONTROL_CHANNELS];
/**
* Mixed servo outputs
*/
uint16_t servos[IO_SERVO_COUNT];
/**
* Relay controls
*/
bool relays[PX4IO_RELAY_CHANNELS];
/**
* If true, we are using the FMU controls, else RC input if available.
*/
bool mixer_manual_override;
/**
* If true, FMU input is available.
*/
bool mixer_fmu_available;
/**
* If true, state that should be reported to FMU has been updated.
*/
bool fmu_report_due;
volatile uint64_t rc_channels_timestamp;
/**
* Last FMU receive time, in microseconds since system boot
*/
uint64_t fmu_data_received_time;
/**
* Current serial interface mode, per the serial_rx_mode parameter
* in the config packet.
*/
uint8_t serial_rx_mode;
/**
* If true, the RC signal has been lost for more than a timeout interval
*/
bool rc_lost;
/**
* If true, the connection to FMU has been lost for more than a timeout interval
*/
bool fmu_lost;
/**
* If true, FMU is ready for autonomous position control. Only used for LED indication
*/
bool vector_flight_mode_ok;
/**
* If true, IO performs an on-board manual override with the RC channel values
*/
bool manual_override_ok;
/*
* Measured battery voltage in mV
*/
uint16_t battery_mv;
/*
* ADC IN5 measurement
*/
uint16_t adc_in5;
/*
* Power supply overcurrent status bits.
*/
uint8_t overcurrent;
volatile uint64_t fmu_data_received_time;
};
extern struct sys_state_s system_state;
#if 0
/*
* Software countdown timers.
*
* Each timer counts down to zero at one tick per ms.
*/
#define TIMER_BLINK_AMBER 0
#define TIMER_BLINK_BLUE 1
#define TIMER_STATUS_PRINT 2
#define TIMER_SANITY 7
#define TIMER_NUM_TIMERS 8
extern volatile int timers[TIMER_NUM_TIMERS];
#endif
/*
* GPIO handling.
*/
@ -206,6 +135,7 @@ extern volatile int timers[TIMER_NUM_TIMERS];
#define ADC_VBATT 4
#define ADC_IN5 5
#define ADC_CHANNEL_COUNT 2
/*
* Mixer
@ -213,34 +143,46 @@ extern volatile int timers[TIMER_NUM_TIMERS];
extern void mixer_tick(void);
extern void mixer_handle_text(const void *buffer, size_t length);
/*
/**
* Safety switch/LED.
*/
extern void safety_init(void);
/*
/**
* FMU communications
*/
extern void comms_main(void) __attribute__((noreturn));
extern void i2c_init(void);
/*
/**
* Register space
*/
extern void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values);
extern int registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values);
/**
* Sensors/misc inputs
*/
extern int adc_init(void);
extern uint16_t adc_measure(unsigned channel);
/*
/**
* R/C receiver handling.
*
* 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(void);
extern bool dsm_input(uint16_t *values, uint16_t *num_values);
extern int sbus_init(const char *device);
extern bool sbus_input(void);
extern bool sbus_input(uint16_t *values, uint16_t *num_values);
/** global debug level for isr_debug() */
extern volatile uint8_t debug_level;
/* send a debug message to the console */
extern void isr_debug(uint8_t level, const char *fmt, ...);
void i2c_dump(void);
void i2c_reset(void);
/*
* Assertion codes
*/
#define A_GPIO_OPEN_FAIL 100
#define A_SERVO_OPEN_FAIL 101
#define A_INPUTQ_OPEN_FAIL 102

644
apps/px4io/registers.c Normal file
View File

@ -0,0 +1,644 @@
/****************************************************************************
*
* Copyright (C) 2012 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 registers.c
*
* Implementation of the PX4IO register space.
*/
#include <nuttx/config.h>
#include <stdbool.h>
#include <stdlib.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
#include "protocol.h"
static int registers_set_one(uint8_t page, uint8_t offset, uint16_t value);
static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate);
/**
* PAGE 0
*
* Static configuration parameters.
*/
static const uint16_t r_page_config[] = {
[PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */
[PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ACTUATOR_COUNT] = IO_SERVO_COUNT,
[PX4IO_P_CONFIG_RC_INPUT_COUNT] = MAX_CONTROL_CHANNELS,
[PX4IO_P_CONFIG_ADC_INPUT_COUNT] = ADC_CHANNEL_COUNT,
[PX4IO_P_CONFIG_RELAY_COUNT] = PX4IO_RELAY_CHANNELS,
};
/**
* PAGE 1
*
* Status values.
*/
uint16_t r_page_status[] = {
[PX4IO_P_STATUS_FREEMEM] = 0,
[PX4IO_P_STATUS_CPULOAD] = 0,
[PX4IO_P_STATUS_FLAGS] = 0,
[PX4IO_P_STATUS_ALARMS] = 0,
[PX4IO_P_STATUS_VBATT] = 0,
[PX4IO_P_STATUS_IBATT] = 0
};
/**
* PAGE 2
*
* Post-mixed actuator values.
*/
uint16_t r_page_actuators[IO_SERVO_COUNT];
/**
* PAGE 3
*
* Servo PWM values
*/
uint16_t r_page_servos[IO_SERVO_COUNT];
/**
* PAGE 4
*
* Raw RC input
*/
uint16_t r_page_raw_rc_input[] =
{
[PX4IO_P_RAW_RC_COUNT] = 0,
[PX4IO_P_RAW_RC_BASE ... (PX4IO_P_RAW_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
};
/**
* PAGE 5
*
* Scaled/routed RC input
*/
uint16_t r_page_rc_input[] = {
[PX4IO_P_RC_VALID] = 0,
[PX4IO_P_RC_BASE ... (PX4IO_P_RC_BASE + MAX_CONTROL_CHANNELS)] = 0
};
/**
* Scratch page; used for registers that are constructed as-read.
*
* PAGE 6 Raw ADC input.
* PAGE 7 PWM rate maps.
*/
uint16_t r_page_scratch[32];
/**
* PAGE 100
*
* Setup registers
*/
volatile uint16_t r_page_setup[] =
{
[PX4IO_P_SETUP_FEATURES] = 0,
[PX4IO_P_SETUP_ARMING] = 0,
[PX4IO_P_SETUP_PWM_RATES] = 0,
[PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50,
[PX4IO_P_SETUP_PWM_ALTRATE] = 200,
[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_SET_DEBUG] = 0,
};
#define PX4IO_P_SETUP_FEATURES_VALID (0)
#define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_ARM_OK | \
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)
#define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1)
#define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1)
/**
* PAGE 101
*
* Control values from the FMU.
*/
volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS];
/*
* PAGE 102 does not have a buffer.
*/
/**
* PAGE 103
*
* R/C channel input configuration.
*/
uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE];
/* 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.
*/
/**
* PAGE 105
*
* Failsafe servo PWM values
*/
uint16_t r_page_servo_failsafe[IO_SERVO_COUNT];
void
registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values)
{
switch (page) {
/* handle bulk controls input */
case PX4IO_PAGE_CONTROLS:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_controls[offset] = *values;
offset++;
num_values--;
values++;
}
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;
break;
/* handle raw PWM input */
case PX4IO_PAGE_DIRECT_PWM:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servos[offset] = *values;
offset++;
num_values--;
values++;
}
system_state.fmu_data_received_time = hrt_absolute_time();
r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM;
break;
/* handle setup for servo failsafe values */
case PX4IO_PAGE_FAILSAFE_PWM:
/* copy channel data */
while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
/* XXX range-check value? */
r_page_servo_failsafe[offset] = *values;
offset++;
num_values--;
values++;
}
break;
/* handle text going to the mixer parser */
case PX4IO_PAGE_MIXERLOAD:
mixer_handle_text(values, num_values * sizeof(*values));
break;
default:
/* avoid offset wrap */
if ((offset + num_values) > 255)
num_values = 255 - offset;
/* iterate individual registers, set each in turn */
while (num_values--) {
if (registers_set_one(page, offset, *values))
break;
offset++;
values++;
}
}
}
static int
registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
{
switch (page) {
case PX4IO_PAGE_STATUS:
switch (offset) {
case PX4IO_P_STATUS_ALARMS:
/* clear bits being written */
r_status_alarms &= ~value;
break;
case PX4IO_P_STATUS_FLAGS:
/*
* Allow FMU override of arming state (to allow in-air restores),
* but only if the arming state is not in sync on the IO side.
*/
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) {
r_status_flags = value;
}
break;
default:
/* just ignore writes to other registers in this page */
break;
}
break;
case PX4IO_PAGE_SETUP:
switch (offset) {
case PX4IO_P_SETUP_FEATURES:
value &= PX4IO_P_SETUP_FEATURES_VALID;
r_setup_features = value;
/* no implemented feature selection at this point */
break;
case PX4IO_P_SETUP_ARMING:
value &= PX4IO_P_SETUP_ARMING_VALID;
/*
* Update arming state - disarm if no longer OK.
* This builds on the requirement that the FMU driver
* asks about the FMU arming state on initialization,
* so that an in-air reset of FMU can not lead to a
* lockup of the IO arming state.
*/
if ((r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && !(value & PX4IO_P_SETUP_ARMING_ARM_OK)) {
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
}
r_setup_arming = value;
break;
case PX4IO_P_SETUP_PWM_RATES:
value &= PX4IO_P_SETUP_RATES_VALID;
pwm_configure_rates(value, r_setup_pwm_defaultrate, r_setup_pwm_altrate);
break;
case PX4IO_P_SETUP_PWM_DEFAULTRATE:
if (value < 50)
value = 50;
if (value > 400)
value = 400;
pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
break;
case PX4IO_P_SETUP_PWM_ALTRATE:
if (value < 50)
value = 50;
if (value > 400)
value = 400;
pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
break;
case PX4IO_P_SETUP_RELAYS:
value &= PX4IO_P_SETUP_RELAYS_VALID;
r_setup_relays = value;
POWER_RELAY1(value & (1 << 0) ? 1 : 0);
POWER_RELAY2(value & (1 << 1) ? 1 : 0);
POWER_ACC1(value & (1 << 2) ? 1 : 0);
POWER_ACC2(value & (1 << 3) ? 1 : 0);
break;
case PX4IO_P_SETUP_SET_DEBUG:
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:
return -1;
}
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];
if (channel >= MAX_CONTROL_CHANNELS)
return -1;
/* disable the channel until we have a chance to sanity-check it */
conf[PX4IO_P_RC_CONFIG_OPTIONS] &= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
switch (index) {
case PX4IO_P_RC_CONFIG_MIN:
case PX4IO_P_RC_CONFIG_CENTER:
case PX4IO_P_RC_CONFIG_MAX:
case PX4IO_P_RC_CONFIG_DEADZONE:
case PX4IO_P_RC_CONFIG_ASSIGNMENT:
conf[index] = value;
break;
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;
/* 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) {
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) {
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 */
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 */
}
break;
/* case PX4IO_RC_PAGE_CONFIG */
}
default:
return -1;
}
return 0;
}
uint8_t last_page;
uint8_t last_offset;
int
registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_values)
{
#define SELECT_PAGE(_page_name) \
do { \
*values = &_page_name[0]; \
*num_values = sizeof(_page_name) / sizeof(_page_name[0]); \
} while(0)
switch (page) {
/*
* Handle pages that are updated dynamically at read time.
*/
case PX4IO_PAGE_STATUS:
/* PX4IO_P_STATUS_FREEMEM */
{
struct mallinfo minfo = mallinfo();
r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.fordblks;
}
/* XXX PX4IO_P_STATUS_CPULOAD */
/* PX4IO_P_STATUS_FLAGS maintained externally */
/* PX4IO_P_STATUS_ALARMS maintained externally */
/* PX4IO_P_STATUS_VBATT */
{
/*
* Coefficients here derived by measurement of the 5-16V
* range on one unit:
*
* V counts
* 5 1001
* 6 1219
* 7 1436
* 8 1653
* 9 1870
* 10 2086
* 11 2303
* 12 2522
* 13 2738
* 14 2956
* 15 3172
* 16 3389
*
* slope = 0.0046067
* intercept = 0.3863
*
* Intercept corrected for best results @ 12V.
*/
unsigned counts = adc_measure(ADC_VBATT);
unsigned mV = (4150 + (counts * 46)) / 10 - 200;
unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
}
/* PX4IO_P_STATUS_IBATT */
{
unsigned counts = adc_measure(ADC_VBATT);
unsigned scaled = (counts * r_page_setup[PX4IO_P_SETUP_IBATT_SCALE]) / 10000;
int corrected = scaled + REG_TO_SIGNED(r_page_setup[PX4IO_P_SETUP_IBATT_BIAS]);
if (corrected < 0)
corrected = 0;
r_page_status[PX4IO_P_STATUS_IBATT] = corrected;
}
SELECT_PAGE(r_page_status);
break;
case PX4IO_PAGE_RAW_ADC_INPUT:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
r_page_scratch[0] = adc_measure(ADC_VBATT);
r_page_scratch[1] = adc_measure(ADC_IN5);
SELECT_PAGE(r_page_scratch);
break;
case PX4IO_PAGE_PWM_INFO:
memset(r_page_scratch, 0, sizeof(r_page_scratch));
for (unsigned i = 0; i < IO_SERVO_COUNT; i++)
r_page_scratch[PX4IO_RATE_MAP_BASE + i] = up_pwm_servo_get_rate_group(i);
SELECT_PAGE(r_page_scratch);
break;
/*
* Pages that are just a straight read of the register state.
*/
/* status pages */
case PX4IO_PAGE_CONFIG:
SELECT_PAGE(r_page_config);
break;
case PX4IO_PAGE_ACTUATORS:
SELECT_PAGE(r_page_actuators);
break;
case PX4IO_PAGE_SERVOS:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_RAW_RC_INPUT:
SELECT_PAGE(r_page_raw_rc_input);
break;
case PX4IO_PAGE_RC_INPUT:
SELECT_PAGE(r_page_rc_input);
break;
/* readback of input pages */
case PX4IO_PAGE_SETUP:
SELECT_PAGE(r_page_setup);
break;
case PX4IO_PAGE_CONTROLS:
SELECT_PAGE(r_page_controls);
break;
case PX4IO_PAGE_RC_CONFIG:
SELECT_PAGE(r_page_rc_input_config);
break;
case PX4IO_PAGE_DIRECT_PWM:
SELECT_PAGE(r_page_servos);
break;
case PX4IO_PAGE_FAILSAFE_PWM:
SELECT_PAGE(r_page_servo_failsafe);
break;
default:
return -1;
}
#undef SELECT_PAGE
#undef COPY_PAGE
last_page = page;
last_offset = offset;
/* if the offset is at or beyond the end of the page, we have no data */
if (offset >= *num_values)
return -1;
/* correct the data pointer and count for the offset */
*values += offset;
*num_values -= offset;
return 0;
}
/*
* Helper function to handle changes to the PWM rate control registers.
*/
static void
pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t altrate)
{
for (unsigned pass = 0; pass < 2; pass++) {
for (unsigned group = 0; group < IO_SERVO_COUNT; group++) {
/* get the channel mask for this rate group */
uint32_t mask = up_pwm_servo_get_rate_group(group);
if (mask == 0)
continue;
/* all channels in the group must be either default or alt-rate */
uint32_t alt = map & mask;
if (pass == 0) {
/* preflight */
if ((alt != 0) && (alt != mask)) {
/* not a legal map, bail with an alarm */
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
return;
}
} else {
/* set it - errors here are unexpected */
if (alt != 0) {
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_altrate) != OK)
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
} else {
if (up_pwm_servo_set_rate_group_update(group, r_setup_pwm_defaultrate) != OK)
r_status_alarms |= PX4IO_P_STATUS_ALARMS_PWM_ERROR;
}
}
}
}
r_setup_pwm_rates = map;
r_setup_pwm_defaultrate = defaultrate;
r_setup_pwm_altrate = altrate;
}

38
apps/px4io/safety.c
View File

@ -36,15 +36,8 @@
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <nuttx/clock.h>
#include <stdbool.h>
#include <drivers/drv_hrt.h>
@ -116,30 +109,28 @@ safety_check_button(void *arg)
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
if (safety_button_pressed && !system_state.armed) {
if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */
system_state.armed = true;
/* switch to armed state */
r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED;
counter++;
system_state.fmu_report_due = true;
}
/* Disarm quickly */
} else if (safety_button_pressed && system_state.armed) {
} else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
system_state.armed = false;
r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED;
counter++;
system_state.fmu_report_due = true;
}
} else {
@ -149,18 +140,18 @@ safety_check_button(void *arg)
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
if (system_state.armed) {
if (system_state.arm_ok) {
if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) {
if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
} else if (system_state.arm_ok) {
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) {
pattern = LED_PATTERN_FMU_ARMED;
} else if (system_state.vector_flight_mode_ok) {
} else if (r_setup_arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) {
pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
@ -185,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 (!system_state.mixer_fmu_available) {
if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) {
failsafe = !failsafe;
} else {
failsafe = false;
}

48
apps/px4io/sbus.c
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;
@ -66,13 +66,13 @@ static unsigned partial_frame_count;
unsigned sbus_frame_drops;
static void sbus_decode(hrt_abstime frame_time);
static bool sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
int
sbus_init(const char *device)
{
if (sbus_fd < 0)
sbus_fd = open(device, O_RDONLY);
sbus_fd = open(device, O_RDONLY | O_NONBLOCK);
if (sbus_fd >= 0) {
struct termios t;
@ -97,7 +97,7 @@ sbus_init(const char *device)
}
bool
sbus_input(void)
sbus_input(uint16_t *values, uint16_t *num_values)
{
ssize_t ret;
hrt_abstime now;
@ -134,7 +134,7 @@ sbus_input(void)
/* if the read failed for any reason, just give up here */
if (ret < 1)
goto out;
return false;
last_rx_time = now;
@ -147,20 +147,14 @@ sbus_input(void)
* If we don't have a full frame, return
*/
if (partial_frame_count < SBUS_FRAME_SIZE)
goto out;
return false;
/*
* Great, it looks like we might have a frame. Go ahead and
* decode it.
*/
sbus_decode(now);
partial_frame_count = 0;
out:
/*
* If we have seen a frame in the last 200ms, we consider ourselves 'locked'
*/
return (now - last_frame_time) < 200000;
return sbus_decode(now, values, num_values);
}
/*
@ -199,13 +193,13 @@ static const struct sbus_bit_pick sbus_decoder[SBUS_INPUT_CHANNELS][3] = {
/* 15 */ { {20, 5, 0x07, 0}, {21, 0, 0xff, 3}, { 0, 0, 0x00, 0} }
};
static void
sbus_decode(hrt_abstime frame_time)
static bool
sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
{
/* check frame boundary markers to avoid out-of-sync cases */
if ((frame[0] != 0x0f) || (frame[24] != 0x00)) {
sbus_frame_drops++;
return;
return false;
}
/* if the failsafe or connection lost bit is set, we consider the frame invalid */
@ -213,8 +207,8 @@ sbus_decode(hrt_abstime frame_time)
(frame[23] & (1 << 3))) { /* failsafe */
/* actively announce signal loss */
system_state.rc_channels = 0;
return 1;
*values = 0;
return false;
}
/* we have received something we think is a frame */
@ -241,23 +235,21 @@ sbus_decode(hrt_abstime frame_time)
}
/* convert 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
system_state.rc_channel_data[channel] = (value / 2) + 998;
values[channel] = (value / 2) + 998;
}
/* 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) */
system_state.rc_channel_data[16] = (frame[23] & (1 << 0)) * 1000 + 998;
values[16] = (frame[23] & (1 << 0)) * 1000 + 998;
/* channel 18 (index 17) */
system_state.rc_channel_data[17] = (frame[23] & (1 << 1)) * 1000 + 998;
values[17] = (frame[23] & (1 << 1)) * 1000 + 998;
}
/* note the number of channels decoded */
system_state.rc_channels = chancount;
*num_values = chancount;
/* and note that we have received data from the R/C controller */
system_state.rc_channels_timestamp = frame_time;
/* trigger an immediate report to the FMU */
system_state.fmu_report_due = true;
return true;
}

2
apps/sdlog/sdlog.c
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},

4
apps/sdlog/sdlog_ringbuffer.h
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

2
apps/sensors/sensor_params.c
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);

110
apps/sensors/sensors.cpp
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;
@ -235,6 +236,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;
@ -480,6 +482,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 */
@ -687,6 +692,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");
@ -990,12 +998,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);
@ -1025,8 +1037,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
@ -1034,24 +1045,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) {
@ -1064,7 +1075,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
}
_last_adc = hrt_absolute_time();
break;
}
}
}
@ -1074,36 +1084,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;
@ -1149,31 +1129,45 @@ Sensors::ppm_poll()
/* Read out values from raw message */
for (unsigned int i = 0; i < channel_limit; i++) {
/* scale around the mid point differently for lower and upper range */
/*
* 1) Constrain to min/max values, as later processing depends on bounds.
*/
if (rc_input.values[i] < _parameters.min[i])
rc_input.values[i] = _parameters.min[i];
if (rc_input.values[i] > _parameters.max[i])
rc_input.values[i] = _parameters.max[i];
/*
* 2) Scale around the mid point differently for lower and upper range.
*
* This is necessary as they don't share the same endpoints and slope.
*
* First normalize to 0..1 range with correct sign (below or above center),
* the total range is 2 (-1..1).
* If center (trim) == min, scale to 0..1, if center (trim) == max,
* scale to -1..0.
*
* As the min and max bounds were enforced in step 1), division by zero
* cannot occur, as for the case of center == min or center == max the if
* statement is mutually exclusive with the arithmetic NaN case.
*
* DO NOT REMOVE OR ALTER STEP 1!
*/
if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) {
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]);
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]);
} else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) {
/* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */
_rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i]));
_rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]);
} else {
/* in the configured dead zone, output zero */
_rc.chan[i].scaled = 0.0f;
}
/* reverse channel if required */
if (i == (int)_rc.function[THROTTLE]) {
if ((int)_parameters.rev[i] == -1) {
_rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled;
}
} else {
_rc.chan[i].scaled *= _parameters.rev[i];
}
_rc.chan[i].scaled *= _parameters.rev[i];
/* handle any parameter-induced blowups */
if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled))
if (!isfinite(_rc.chan[i].scaled))
_rc.chan[i].scaled = 0.0f;
}

20
apps/system/readline/readline.c
View File

@ -126,7 +126,7 @@ static inline int readline_rawgetc(int infd)
* error occurs).
*/
do
for (;;)
{
/* Read one character from the incoming stream */
@ -154,13 +154,21 @@ static inline int readline_rawgetc(int infd)
{
return -errcode;
}
continue;
}
else if (buffer == '\0')
{
/* Ignore NUL characters, since they look like EOF to our caller */
continue;
}
/* Success, return the character that was read */
return (int)buffer;
}
while (nread < 1);
/* On success, return the character that was read */
return (int)buffer;
}
/****************************************************************************

42
apps/systemcmds/i2c/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Build the i2c test tool.
#
APPNAME = i2c
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk

140
apps/systemcmds/i2c/i2c.c Normal file
View File

@ -0,0 +1,140 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file i2c.c
*
* i2c hacking tool
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/i2c.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
#ifndef PX4_I2C_BUS_ONBOARD
# error PX4_I2C_BUS_ONBOARD not defined, no device interface
#endif
#ifndef PX4_I2C_OBDEV_PX4IO
# error PX4_I2C_OBDEV_PX4IO not defined
#endif
__EXPORT int i2c_main(int argc, char *argv[]);
static int transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len);
static struct i2c_dev_s *i2c;
int i2c_main(int argc, char *argv[])
{
/* find the right I2C */
i2c = up_i2cinitialize(PX4_I2C_BUS_ONBOARD);
if (i2c == NULL)
errx(1, "failed to locate I2C bus");
usleep(100000);
uint8_t buf[] = { 0, 4};
int ret = transfer(PX4_I2C_OBDEV_PX4IO, buf, sizeof(buf), NULL, 0);
if (ret)
errx(1, "send failed - %d", ret);
uint32_t val;
ret = transfer(PX4_I2C_OBDEV_PX4IO, NULL, 0, (uint8_t *)&val, sizeof(val));
if (ret)
errx(1, "recive failed - %d", ret);
errx(0, "got 0x%08x", val);
}
static int
transfer(uint8_t address, uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
{
struct i2c_msg_s msgv[2];
unsigned msgs;
int ret;
// debug("transfer out %p/%u in %p/%u", send, send_len, recv, recv_len);
msgs = 0;
if (send_len > 0) {
msgv[msgs].addr = address;
msgv[msgs].flags = 0;
msgv[msgs].buffer = send;
msgv[msgs].length = send_len;
msgs++;
}
if (recv_len > 0) {
msgv[msgs].addr = address;
msgv[msgs].flags = I2C_M_READ;
msgv[msgs].buffer = recv;
msgv[msgs].length = recv_len;
msgs++;
}
if (msgs == 0)
return -1;
/*
* I2C architecture means there is an unavoidable race here
* if there are any devices on the bus with a different frequency
* preference. Really, this is pointless.
*/
I2C_SETFREQUENCY(i2c, 400000);
ret = I2C_TRANSFER(i2c, &msgv[0], msgs);
// reset the I2C bus to unwedge on error
if (ret != OK)
up_i2creset(i2c);
return ret;
}

18
apps/systemcmds/mixer/mixer.c
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))

8
apps/systemcmds/preflight_check/preflight_check.c
View File

@ -54,6 +54,8 @@
#include <drivers/drv_accel.h>
#include <drivers/drv_baro.h>
#include <mavlink/mavlink_log.h>
__EXPORT int preflight_check_main(int argc, char *argv[]);
static int led_toggle(int leds, int led);
static int led_on(int leds, int led);
@ -75,6 +77,8 @@ int preflight_check_main(int argc, char *argv[])
bool system_ok = true;
int fd;
/* open text message output path */
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
int ret;
/* give the system some time to sample the sensors in the background */
@ -86,6 +90,7 @@ int preflight_check_main(int argc, char *argv[])
fd = open(MAG_DEVICE_PATH, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
system_ok = false;
goto system_eval;
}
@ -93,6 +98,7 @@ int preflight_check_main(int argc, char *argv[])
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CALIBRATION");
system_ok = false;
goto system_eval;
}
@ -105,6 +111,7 @@ int preflight_check_main(int argc, char *argv[])
if (ret != OK) {
warnx("accel self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK");
system_ok = false;
goto system_eval;
}
@ -117,6 +124,7 @@ int preflight_check_main(int argc, char *argv[])
if (ret != OK) {
warnx("gyro self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK");
system_ok = false;
goto system_eval;
}

42
apps/systemcmds/pwm/Makefile Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2012 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.
#
############################################################################
#
# Build the pwm tool.
#
APPNAME = pwm
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 4096
include $(APPDIR)/mk/app.mk

233
apps/systemcmds/pwm/pwm.c Normal file
View File

@ -0,0 +1,233 @@
/****************************************************************************
*
* 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 pwm.c
*
* PWM servo output configuration and monitoring tool.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <nuttx/i2c.h>
#include <nuttx/mtd.h>
#include <nuttx/fs/nxffs.h>
#include <nuttx/fs/ioctl.h>
#include <arch/board/board.h>
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
#include "drivers/drv_pwm_output.h"
static void usage(const char *reason);
__EXPORT int pwm_main(int argc, char *argv[]);
static void
usage(const char *reason)
{
if (reason != NULL)
warnx("%s", reason);
errx(1,
"usage:\n"
"pwm [-v] [-d <device>] [-u <alt_rate>] [-c <channel group>] [arm|disarm] [<channel_value> ...]\n"
" -v Print information about the PWM device\n"
" <device> PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
" <alt_rate> PWM update rate for channels in <alt_channel_mask>\n"
" <channel_group> Channel group that should update at the alternate rate (may be specified more than once)\n"
" arm | disarm Arm or disarm the ouptut\n"
" <channel_value>... PWM output values in microseconds to assign to the PWM outputs\n"
"\n"
"When -c is specified, any channel groups not listed with -c will update at the default rate.\n"
);
}
int
pwm_main(int argc, char *argv[])
{
const char *dev = PWM_OUTPUT_DEVICE_PATH;
unsigned alt_rate = 0;
uint32_t alt_channel_groups = 0;
bool alt_channels_set = false;
bool print_info = false;
int ch;
int ret;
char *ep;
unsigned group;
if (argc < 2)
usage(NULL);
while ((ch = getopt(argc, argv, "c:d:u:v")) != EOF) {
switch (ch) {
case 'c':
group = strtoul(optarg, &ep, 0);
if ((*ep != '\0') || (group >= 32))
usage("bad channel_group value");
alt_channel_groups |= (1 << group);
alt_channels_set = true;
break;
case 'd':
dev = optarg;
break;
case 'u':
alt_rate = strtol(optarg, &ep, 0);
if (*ep != '\0')
usage("bad alt_rate value");
break;
case 'v':
print_info = true;
break;
default:
usage(NULL);
}
}
argc -= optind;
argv += optind;
/* open for ioctl only */
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
/* change alternate PWM rate */
if (alt_rate > 0) {
ret = ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, alt_rate);
if (ret != OK)
err(1, "PWM_SERVO_SET_UPDATE_RATE (check rate for sanity)");
}
/* assign alternate rate to channel groups */
if (alt_channels_set) {
uint32_t mask = 0;
for (unsigned group = 0; group < 32; group++) {
if ((1 << group) & alt_channel_groups) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(group), (unsigned long)&group_mask);
if (ret != OK)
err(1, "PWM_SERVO_GET_RATEGROUP(%u)", group);
mask |= group_mask;
}
}
ret = ioctl(fd, PWM_SERVO_SELECT_UPDATE_RATE, mask);
if (ret != OK)
err(1, "PWM_SERVO_SELECT_UPDATE_RATE");
}
/* iterate remaining arguments */
unsigned channel = 0;
while (argc--) {
const char *arg = argv[0];
argv++;
if (!strcmp(arg, "arm")) {
ret = ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
continue;
}
if (!strcmp(arg, "disarm")) {
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
continue;
}
unsigned pwm_value = strtol(arg, &ep, 0);
if (*ep == '\0') {
ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value);
if (ret != OK)
err(1, "PWM_SERVO_SET(%d)", channel);
channel++;
continue;
}
usage("unrecognised option");
}
/* print verbose info */
if (print_info) {
/* get the number of servo channels */
unsigned count;
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&count);
if (ret != OK)
err(1, "PWM_SERVO_GET_COUNT");
/* print current servo values */
for (unsigned i = 0; i < count; i++) {
servo_position_t spos;
ret = ioctl(fd, PWM_SERVO_GET(i), (unsigned long)&spos);
if (ret == OK) {
printf("channel %u: %uus\n", i, spos);
} else {
printf("%u: ERROR\n", i);
}
}
/* print rate groups */
for (unsigned i = 0; i < count; i++) {
uint32_t group_mask;
ret = ioctl(fd, PWM_SERVO_GET_RATEGROUP(i), (unsigned long)&group_mask);
if (ret != OK)
break;
if (group_mask != 0) {
printf("channel group %u: channels", i);
for (unsigned j = 0; j < 32; j++)
if (group_mask & (1 << j))
printf(" %u", j);
printf("\n");
}
}
fflush(stdout);
}
exit(0);
}

54
apps/systemlib/airspeed.c
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);
}
}

69
apps/systemlib/airspeed.h
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

2
apps/systemlib/conversions.c
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));
}

8
apps/systemlib/conversions.h
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

1
apps/systemlib/mixer/mixer.cpp
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)
{

2
apps/systemlib/mixer/mixer.h
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.

1
apps/systemlib/mixer/mixer_group.cpp
View File

@ -93,6 +93,7 @@ MixerGroup::reset()
mixer = _first;
_first = mixer->_next;
delete mixer;
mixer = nullptr;
}
}

2
apps/systemlib/param/param.c
View File

@ -87,7 +87,7 @@ struct param_wbuf_s {
UT_array *param_values;
/** array info for the modified parameters array */
UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
const UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL};
/** parameter update topic */
ORB_DEFINE(parameter_update, struct parameter_update_s);

3
apps/uORB/objects_common.cpp
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);

4
apps/uORB/topics/actuator_controls_effective.h
View File

@ -37,8 +37,8 @@
* Actuator control topics - mixer inputs.
*
* Values published to these topics are the outputs of the vehicle control
* system, and are expected to be mixed and used to drive the actuators
* (servos, speed controls, etc.) that operate the vehicle.
* system and mixing process; they are the control-scale values that are
* then fed to the actual actuator driver.
*
* Each topic can be published by a single controller
*/

9
apps/uORB/topics/differential_pressure.h
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

3
apps/uORB/topics/subsystem_info.h
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
};
/**

9
apps/uORB/topics/vehicle_status.h
View File

@ -156,7 +156,9 @@ struct vehicle_status_s
enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */
enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */
enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */
int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */
int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */
int32_t system_id; /**< system id, inspired by MAVLink's system ID field */
int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */
/* system flags - these represent the state predicates */
@ -171,8 +173,9 @@ struct vehicle_status_s
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_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 */
@ -209,6 +212,8 @@ struct vehicle_status_s
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 */
bool flag_valid_launch_position; /**< indicates a valid launch position */
bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */
bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */
};
/**

7
apps/uORB/uORB.cpp
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 */

5
makefiles/firmware.mk
View File

@ -241,6 +241,11 @@ include $(PX4_MK_DIR)/nuttx.mk
ifneq ($(ROMFS_ROOT),)
#
# Note that there is no support for more than one root directory or constructing
# a root from several templates. That would be a nice feature.
#
# Add dependencies on anything in the ROMFS root
ROMFS_DEPS += $(wildcard \
(ROMFS_ROOT)/* \

27
nuttx/arch/arm/src/stm32/stm32_i2c.c
View File

@ -185,6 +185,22 @@ enum stm32_trace_e
I2CEVENT_ERROR /* Error occurred, param = 0 */
};
#ifdef CONFIG_I2C_TRACE
static const char *stm32_trace_names[] = {
"NONE ",
"SENDADDR ",
"SENDBYTE ",
"ITBUFEN ",
"RCVBYTE ",
"REITBUFEN ",
"DISITBUFEN",
"BTFNOSTART",
"BTFRESTART",
"BTFSTOP ",
"ERROR "
};
#endif
/* Trace data */
struct stm32_trace_s
@ -846,6 +862,7 @@ static void stm32_i2c_traceevent(FAR struct stm32_i2c_priv_s *priv,
trace->event = event;
trace->parm = parm;
trace->time = clock_systimer();
/* Bump up the trace index (unless we are out of trace entries) */
@ -869,8 +886,8 @@ static void stm32_i2c_tracedump(FAR struct stm32_i2c_priv_s *priv)
for (i = 0; i <= priv->tndx; i++)
{
trace = &priv->trace[i];
syslog("%2d. STATUS: %08x COUNT: %3d EVENT: %2d PARM: %08x TIME: %d\n",
i+1, trace->status, trace->count, trace->event, trace->parm,
syslog("%2d. STATUS: %08x COUNT: %3d EVENT: %s PARM: %08x TIME: %d\n",
i+1, trace->status, trace->count, stm32_trace_names[trace->event], trace->parm,
trace->time - priv->start_time);
}
}
@ -1620,8 +1637,8 @@ static int stm32_i2c_process(FAR struct i2c_dev_s *dev, FAR struct i2c_msg_s *ms
status = stm32_i2c_getstatus(priv);
errval = ETIMEDOUT;
i2cdbg("Timed out: CR1: %04x status: %08x\n",
stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET), status);
i2cdbg("Timed out: CR1: %04x status: %08x after %d\n",
stm32_i2c_getreg(priv, STM32_I2C_CR1_OFFSET), status, timeout_us);
/* "Note: When the STOP, START or PEC bit is set, the software must
* not perform any write access to I2C_CR1 before this bit is
@ -2005,7 +2022,7 @@ int up_i2creset(FAR struct i2c_dev_s * dev)
/* Give up if we have tried too hard */
if (clock_count++ > 1000)
if (clock_count++ > CONFIG_STM32_I2CTIMEOTICKS)
{
goto out;
}

160
nuttx/arch/arm/src/stm32/stm32_otgfsdev.c
View File

@ -397,7 +397,6 @@ struct stm32_ep_s
struct stm32_req_s *tail;
uint8_t epphy; /* Physical EP address */
uint8_t eptype:2; /* Endpoint type */
uint8_t configured:1; /* 1: Endpoint has been configured */
uint8_t active:1; /* 1: A request is being processed */
uint8_t stalled:1; /* 1: Endpoint is stalled */
uint8_t isin:1; /* 1: IN Endpoint */
@ -679,6 +678,99 @@ static const struct usbdev_ops_s g_devops =
.pullup = stm32_pullup,
};
/* Device error strings that may be enabled for more desciptive USB trace
* output.
*/
#ifdef CONFIG_USBDEV_TRACE_STRINGS
const struct trace_msg_t g_usb_trace_strings_deverror[] =
{
TRACE_STR(STM32_TRACEERR_ALLOCFAIL ),
TRACE_STR(STM32_TRACEERR_BADCLEARFEATURE ),
TRACE_STR(STM32_TRACEERR_BADDEVGETSTATUS ),
TRACE_STR(STM32_TRACEERR_BADEPNO ),
TRACE_STR(STM32_TRACEERR_BADEPGETSTATUS ),
TRACE_STR(STM32_TRACEERR_BADGETCONFIG ),
TRACE_STR(STM32_TRACEERR_BADGETSETDESC ),
TRACE_STR(STM32_TRACEERR_BADGETSTATUS ),
TRACE_STR(STM32_TRACEERR_BADSETADDRESS ),
TRACE_STR(STM32_TRACEERR_BADSETCONFIG ),
TRACE_STR(STM32_TRACEERR_BADSETFEATURE ),
TRACE_STR(STM32_TRACEERR_BADTESTMODE ),
TRACE_STR(STM32_TRACEERR_BINDFAILED ),
TRACE_STR(STM32_TRACEERR_DISPATCHSTALL ),
TRACE_STR(STM32_TRACEERR_DRIVER ),
TRACE_STR(STM32_TRACEERR_DRIVERREGISTERED),
TRACE_STR(STM32_TRACEERR_EP0NOSETUP ),
TRACE_STR(STM32_TRACEERR_EP0SETUPSTALLED ),
TRACE_STR(STM32_TRACEERR_EPINNULLPACKET ),
TRACE_STR(STM32_TRACEERR_EPOUTNULLPACKET ),
TRACE_STR(STM32_TRACEERR_INVALIDCTRLREQ ),
TRACE_STR(STM32_TRACEERR_INVALIDPARMS ),
TRACE_STR(STM32_TRACEERR_IRQREGISTRATION ),
TRACE_STR(STM32_TRACEERR_NOEP ),
TRACE_STR(STM32_TRACEERR_NOTCONFIGURED ),
TRACE_STR(STM32_TRACEERR_EPOUTQEMPTY ),
TRACE_STR(STM32_TRACEERR_EPINREQEMPTY ),
TRACE_STR(STM32_TRACEERR_NOOUTSETUP ),
TRACE_STR(STM32_TRACEERR_POLLTIMEOUT ),
TRACE_STR_END
};
#endif
/* Interrupt event strings that may be enabled for more desciptive USB trace
* output.
*/
#ifdef CONFIG_USBDEV_TRACE_STRINGS
const struct trace_msg_t g_usb_trace_strings_intdecode[] =
{
TRACE_STR(STM32_TRACEINTID_USB ),
TRACE_STR(STM32_TRACEINTID_INTPENDING ),
TRACE_STR(STM32_TRACEINTID_EPOUT ),
TRACE_STR(STM32_TRACEINTID_EPIN ),
TRACE_STR(STM32_TRACEINTID_MISMATCH ),
TRACE_STR(STM32_TRACEINTID_WAKEUP ),
TRACE_STR(STM32_TRACEINTID_SUSPEND ),
TRACE_STR(STM32_TRACEINTID_SOF ),
TRACE_STR(STM32_TRACEINTID_RXFIFO ),
TRACE_STR(STM32_TRACEINTID_DEVRESET ),
TRACE_STR(STM32_TRACEINTID_ENUMDNE ),
TRACE_STR(STM32_TRACEINTID_IISOIXFR ),
TRACE_STR(STM32_TRACEINTID_IISOOXFR ),
TRACE_STR(STM32_TRACEINTID_SRQ ),
TRACE_STR(STM32_TRACEINTID_OTG ),
TRACE_STR(STM32_TRACEINTID_EPOUT_XFRC ),
TRACE_STR(STM32_TRACEINTID_EPOUT_EPDISD),
TRACE_STR(STM32_TRACEINTID_EPOUT_SETUP ),
TRACE_STR(STM32_TRACEINTID_DISPATCH ),
TRACE_STR(STM32_TRACEINTID_GETSTATUS ),
TRACE_STR(STM32_TRACEINTID_EPGETSTATUS ),
TRACE_STR(STM32_TRACEINTID_DEVGETSTATUS),
TRACE_STR(STM32_TRACEINTID_IFGETSTATUS ),
TRACE_STR(STM32_TRACEINTID_CLEARFEATURE),
TRACE_STR(STM32_TRACEINTID_SETFEATURE ),
TRACE_STR(STM32_TRACEINTID_SETADDRESS ),
TRACE_STR(STM32_TRACEINTID_GETSETDESC ),
TRACE_STR(STM32_TRACEINTID_GETCONFIG ),
TRACE_STR(STM32_TRACEINTID_SETCONFIG ),
TRACE_STR(STM32_TRACEINTID_GETSETIF ),
TRACE_STR(STM32_TRACEINTID_SYNCHFRAME ),
TRACE_STR(STM32_TRACEINTID_EPIN_XFRC ),
TRACE_STR(STM32_TRACEINTID_EPIN_TOC ),
TRACE_STR(STM32_TRACEINTID_EPIN_ITTXFE ),
TRACE_STR(STM32_TRACEINTID_EPIN_EPDISD ),
TRACE_STR(STM32_TRACEINTID_EPIN_TXFE ),
TRACE_STR(STM32_TRACEINTID_EPIN_EMPWAIT),
TRACE_STR(STM32_TRACEINTID_OUTNAK ),
TRACE_STR(STM32_TRACEINTID_OUTRECVD ),
TRACE_STR(STM32_TRACEINTID_OUTDONE ),
TRACE_STR(STM32_TRACEINTID_SETUPDONE ),
TRACE_STR(STM32_TRACEINTID_SETUPRECVD ),
TRACE_STR_END
};
#endif
/*******************************************************************************
* Public Data
*******************************************************************************/
@ -1142,7 +1234,7 @@ static void stm32_epin_request(FAR struct stm32_usbdev_s *priv,
/* Add one more packet to the TxFIFO. We will wait for the transfer
* complete event before we add the next packet (or part of a packet
* to the TxFIFO).
*
*
* The documentation says that we can can multiple packets to the TxFIFO,
* but it seems that we need to get the transfer complete event before
* we can add the next (or maybe I have got something wrong?)
@ -1796,13 +1888,6 @@ static struct stm32_ep_s *stm32_ep_findbyaddr(struct stm32_usbdev_s *priv,
privep = &priv->epout[epphy];
}
/* Verify that the endpoint has been configured */
if (!privep->configured)
{
return NULL;
}
/* Return endpoint reference */
DEBUGASSERT(privep->epphy == epphy);
@ -2459,16 +2544,16 @@ static inline void stm32_epout(FAR struct stm32_usbdev_s *priv, uint8_t epno)
/* Continue processing data from the EP0 OUT request queue */
stm32_epout_complete(priv, privep);
}
/* If we are not actively processing an OUT request, then we
* need to setup to receive the next control request.
*/
/* If we are not actively processing an OUT request, then we
* need to setup to receive the next control request.
*/
if (!privep->active)
{
stm32_ep0out_ctrlsetup(priv);
priv->ep0state = EP0STATE_IDLE;
if (!privep->active)
{
stm32_ep0out_ctrlsetup(priv);
priv->ep0state = EP0STATE_IDLE;
}
}
}
@ -2626,16 +2711,16 @@ static inline void stm32_epin(FAR struct stm32_usbdev_s *priv, uint8_t epno)
/* Continue processing data from the EP0 OUT request queue */
stm32_epin_request(priv, privep);
}
/* If we are not actively processing an OUT request, then we
* need to setup to receive the next control request.
*/
/* If we are not actively processing an OUT request, then we
* need to setup to receive the next control request.
*/
if (!privep->active)
{
stm32_ep0out_ctrlsetup(priv);
priv->ep0state = EP0STATE_IDLE;
if (!privep->active)
{
stm32_ep0out_ctrlsetup(priv);
priv->ep0state = EP0STATE_IDLE;
}
}
/* Test mode is another special case */
@ -2754,7 +2839,7 @@ static inline void stm32_epin_interrupt(FAR struct stm32_usbdev_s *priv)
* interrupt here; it will be re-enabled if there is still
* insufficient space in the TxFIFO.
*/
empty &= ~OTGFS_DIEPEMPMSK(epno);
stm32_putreg(empty, STM32_OTGFS_DIEPEMPMSK);
stm32_putreg(OTGFS_DIEPINT_XFRC, STM32_OTGFS_DIEPINT(epno));
@ -3063,6 +3148,12 @@ static inline void stm32_rxinterrupt(FAR struct stm32_usbdev_s *priv)
datlen = GETUINT16(priv->ctrlreq.len);
if (USB_REQ_ISOUT(priv->ctrlreq.type) && datlen > 0)
{
/* Clear NAKSTS so that we can receive the data */
regval = stm32_getreg(STM32_OTGFS_DOEPCTL0);
regval |= OTGFS_DOEPCTL0_CNAK;
stm32_putreg(regval, STM32_OTGFS_DOEPCTL0);
/* Wait for the data phase. */
priv->ep0state = EP0STATE_SETUP_OUT;
@ -3654,7 +3745,7 @@ static int stm32_epout_configure(FAR struct stm32_ep_s *privep, uint8_t eptype,
{
regval |= OTGFS_DOEPCTL_CNAK;
}
regval &= ~(OTGFS_DOEPCTL_MPSIZ_MASK | OTGFS_DOEPCTL_EPTYP_MASK);
regval |= mpsiz;
regval |= (eptype << OTGFS_DOEPCTL_EPTYP_SHIFT);
@ -3750,7 +3841,7 @@ static int stm32_epin_configure(FAR struct stm32_ep_s *privep, uint8_t eptype,
{
regval |= OTGFS_DIEPCTL_CNAK;
}
regval &= ~(OTGFS_DIEPCTL_MPSIZ_MASK | OTGFS_DIEPCTL_EPTYP_MASK | OTGFS_DIEPCTL_TXFNUM_MASK);
regval |= mpsiz;
regval |= (eptype << OTGFS_DIEPCTL_EPTYP_SHIFT);
@ -4345,19 +4436,6 @@ static int stm32_epin_setstall(FAR struct stm32_ep_s *privep)
regaddr = STM32_OTGFS_DIEPCTL(privep->epphy);
regval = stm32_getreg(regaddr);
/* Is the endpoint enabled? */
if ((regval & OTGFS_DIEPCTL_EPENA) != 0)
{
/* Yes.. the endpoint is enabled, disable it */
regval = OTGFS_DIEPCTL_EPDIS;
}
else
{
regval = 0;
}
/* Then stall the endpoint */
regval |= OTGFS_DIEPCTL_STALL;

183
nuttx/arch/arm/src/stm32/stm32_usbdev.c
View File

@ -1,7 +1,7 @@
/****************************************************************************
* arch/arm/src/stm32/stm32_usbdev.c
*
* Copyright (C) 2009-2012 Gregory Nutt. All rights reserved.
* Copyright (C) 2009-2013 Gregory Nutt. All rights reserved.
* Author: Gregory Nutt <gnutt@nuttx.orgr>
*
* References:
@ -59,7 +59,9 @@
#include <arch/irq.h>
#include "up_arch.h"
#include "stm32_internal.h"
#include "stm32.h"
#include "stm32_syscfg.h"
#include "stm32_gpio.h"
#include "stm32_usbdev.h"
#if defined(CONFIG_USBDEV) && defined(CONFIG_STM32_USB)
@ -78,6 +80,20 @@
# define CONFIG_USB_PRI NVIC_SYSH_PRIORITY_DEFAULT
#endif
/* USB Interrupts. Should be re-mapped if CAN is used. */
#ifdef CONFIG_STM32_STM32F30XX
# ifdef CONFIG_STM32_USB_ITRMP
# define STM32_IRQ_USBHP STM32_IRQ_USBHP_2
# define STM32_IRQ_USBLP STM32_IRQ_USBLP_2
# define STM32_IRQ_USBWKUP STM32_IRQ_USBWKUP_2
# else
# define STM32_IRQ_USBHP STM32_IRQ_USBHP_1
# define STM32_IRQ_USBLP STM32_IRQ_USBLP_1
# define STM32_IRQ_USBWKUP STM32_IRQ_USBWKUP_1
# endif
#endif
/* Extremely detailed register debug that you would normally never want
* enabled.
*/
@ -250,12 +266,12 @@
/* The various states of a control pipe */
enum stm32_devstate_e
enum stm32_ep0state_e
{
DEVSTATE_IDLE = 0, /* No request in progress */
DEVSTATE_RDREQUEST, /* Read request in progress */
DEVSTATE_WRREQUEST, /* Write request in progress */
DEVSTATE_STALLED /* We are stalled */
EP0STATE_IDLE = 0, /* No request in progress */
EP0STATE_RDREQUEST, /* Read request in progress */
EP0STATE_WRREQUEST, /* Write request in progress */
EP0STATE_STALLED /* We are stalled */
};
/* Resume states */
@ -320,7 +336,7 @@ struct stm32_usbdev_s
/* STM32-specific fields */
struct usb_ctrlreq_s ctrl; /* Last EP0 request */
uint8_t devstate; /* Driver state (see enum stm32_devstate_e) */
uint8_t ep0state; /* State of EP0 (see enum stm32_ep0state_e) */
uint8_t rsmstate; /* Resume state (see enum stm32_rsmstate_e) */
uint8_t nesofs; /* ESOF counter (for resume support) */
uint8_t rxpending:1; /* 1: OUT data in PMA, but no read requests */
@ -1014,7 +1030,7 @@ static void stm32_copytopma(const uint8_t *buffer, uint16_t pma, uint16_t nbytes
/* Copy loop. Source=user buffer, Dest=packet memory */
dest = (uint16_t*)(STM32_USBCANRAM_BASE + ((uint32_t)pma << 1));
dest = (uint16_t*)(STM32_USBRAM_BASE + ((uint32_t)pma << 1));
for (i = nwords; i != 0; i--)
{
/* Read two bytes and pack into on 16-bit word */
@ -1044,7 +1060,7 @@ stm32_copyfrompma(uint8_t *buffer, uint16_t pma, uint16_t nbytes)
/* Copy loop. Source=packet memory, Dest=user buffer */
src = (uint32_t*)(STM32_USBCANRAM_BASE + ((uint32_t)pma << 1));
src = (uint32_t*)(STM32_USBRAM_BASE + ((uint32_t)pma << 1));
for (i = nwords; i != 0; i--)
{
/* Copy 16-bits from packet memory to user buffer. */
@ -1142,7 +1158,7 @@ static void stm32_reqcomplete(struct stm32_ep_s *privep, int16_t result)
bool stalled = privep->stalled;
if (USB_EPNO(privep->ep.eplog) == EP0)
{
privep->stalled = (privep->dev->devstate == DEVSTATE_STALLED);
privep->stalled = (privep->dev->ep0state == EP0STATE_STALLED);
}
/* Save the result in the request structure */
@ -1222,8 +1238,7 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive
*/
usbtrace(TRACE_INTDECODE(STM32_TRACEINTID_EPINQEMPTY), 0);
priv->devstate = DEVSTATE_IDLE;
return OK;
return -ENOENT;
}
epno = USB_EPNO(privep->ep.eplog);
@ -1267,7 +1282,6 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive
buf = privreq->req.buf + privreq->req.xfrd;
stm32_epwrite(priv, privep, buf, nbytes);
priv->devstate = DEVSTATE_WRREQUEST;
/* Update for the next data IN interrupt */
@ -1275,15 +1289,16 @@ static int stm32_wrrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive
bytesleft = privreq->req.len - privreq->req.xfrd;
/* If all of the bytes were sent (including any final null packet)
* then we are finished with the transfer
* then we are finished with the request buffer).
*/
if (bytesleft == 0 && !privep->txnullpkt)
{
/* Return the write request to the class driver */
usbtrace(TRACE_COMPLETE(USB_EPNO(privep->ep.eplog)), privreq->req.xfrd);
privep->txnullpkt = 0;
stm32_reqcomplete(privep, OK);
priv->devstate = DEVSTATE_IDLE;
}
return OK;
@ -1314,7 +1329,7 @@ static int stm32_rdrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive
*/
usbtrace(TRACE_INTDECODE(STM32_TRACEINTID_EPOUTQEMPTY), epno);
return OK;
return -ENOENT;
}
ullvdbg("EP%d: len=%d xfrd=%d\n", epno, privreq->req.len, privreq->req.xfrd);
@ -1343,22 +1358,18 @@ static int stm32_rdrequest(struct stm32_usbdev_s *priv, struct stm32_ep_s *prive
/* Receive the next packet */
stm32_copyfrompma(dest, src, readlen);
priv->devstate = DEVSTATE_RDREQUEST;
/* If the receive buffer is full or this is a partial packet,
* then we are finished with the transfer
* then we are finished with the request buffer).
*/
privreq->req.xfrd += readlen;
if (pmalen < privep->ep.maxpacket || privreq->req.xfrd >= privreq->req.len)
{
/* Complete the transfer and mark the state IDLE. The endpoint
* RX will be marked valid when the data phase completes.
*/
/* Return the read request to the class driver. */
usbtrace(TRACE_COMPLETE(epno), privreq->req.xfrd);
stm32_reqcomplete(privep, OK);
priv->devstate = DEVSTATE_IDLE;
}
return OK;
@ -1400,7 +1411,7 @@ static void stm32_dispatchrequest(struct stm32_usbdev_s *priv)
/* Stall on failure */
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_DISPATCHSTALL), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
}
@ -1436,7 +1447,7 @@ static void stm32_epdone(struct stm32_usbdev_s *priv, uint8_t epno)
{
/* Read host data into the current read request */
stm32_rdrequest(priv, privep);
(void)stm32_rdrequest(priv, privep);
/* "After the received data is processed, the application software
* should set the STAT_RX bits to '11' (Valid) in the USB_EPnR,
@ -1567,7 +1578,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
ullvdbg("SETUP: type=%02x req=%02x value=%04x index=%04x len=%04x\n",
priv->ctrl.type, priv->ctrl.req, value.w, index.w, len.w);
priv->devstate = DEVSTATE_IDLE;
priv->ep0state = EP0STATE_IDLE;
/* Dispatch any non-standard requests */
@ -1600,7 +1611,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
index.b[MSB] != 0 || value.w != 0)
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADEPGETSTATUS), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
else
{
@ -1613,7 +1624,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
if (epno >= STM32_NENDPOINTS)
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADEPGETSTATUS), epno);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
else
{
@ -1663,7 +1674,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
else
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADDEVGETSTATUS), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
break;
@ -1679,7 +1690,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
default:
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETSTATUS), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
break;
}
@ -1720,7 +1731,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
else
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADCLEARFEATURE), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
}
@ -1764,7 +1775,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
else
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETFEATURE), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
}
@ -1783,7 +1794,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
index.w != 0 || len.w != 0 || value.w > 127)
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETADDRESS), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
/* Note that setting of the device address will be deferred. A zero-length
@ -1818,7 +1829,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
else
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETSETDESC), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
break;
@ -1843,7 +1854,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
else
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADGETCONFIG), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
break;
@ -1868,7 +1879,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
else
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_BADSETCONFIG), 0);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
break;
@ -1910,7 +1921,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
default:
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_INVALIDCTRLREQ), priv->ctrl.req);
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
break;
}
@ -1922,9 +1933,9 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
* must be sent (may be a zero length packet).
* 2. The request was successfully handled by the class implementation. In
* case, the EP0 IN response has already been queued and the local variable
* 'handled' will be set to true and devstate != DEVSTATE_STALLED;
* 'handled' will be set to true and ep0state != EP0STATE_STALLED;
* 3. An error was detected in either the above logic or by the class implementation
* logic. In either case, priv->state will be set DEVSTATE_STALLED
* logic. In either case, priv->state will be set EP0STATE_STALLED
* to indicate this case.
*
* NOTE: Non-standard requests are a special case. They are handled by the
@ -1932,7 +1943,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
* logic altogether.
*/
if (priv->devstate != DEVSTATE_STALLED && !handled)
if (priv->ep0state != EP0STATE_STALLED && !handled)
{
/* We will response. First, restrict the data length to the length
* requested in the setup packet
@ -1946,7 +1957,7 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
/* Send the response (might be a zero-length packet) */
stm32_epwrite(priv, ep0, response.b, nbytes);
priv->devstate = DEVSTATE_IDLE;
priv->ep0state = EP0STATE_IDLE;
}
}
@ -1956,6 +1967,8 @@ static void stm32_ep0setup(struct stm32_usbdev_s *priv)
static void stm32_ep0in(struct stm32_usbdev_s *priv)
{
int ret;
/* There is no longer anything in the EP0 TX packet memory */
priv->eplist[EP0].txbusy = false;
@ -1964,14 +1977,15 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv)
* from the class driver?
*/
if (priv->devstate == DEVSTATE_WRREQUEST)
if (priv->ep0state == EP0STATE_WRREQUEST)
{
stm32_wrrequest(priv, &priv->eplist[EP0]);
ret = stm32_wrrequest(priv, &priv->eplist[EP0]);
priv->ep0state = ((ret == OK) ? EP0STATE_WRREQUEST : EP0STATE_IDLE);
}
/* No.. Are we processing the completion of a status response? */
else if (priv->devstate == DEVSTATE_IDLE)
else if (priv->ep0state == EP0STATE_IDLE)
{
/* Look at the saved SETUP command. Was it a SET ADDRESS request?
* If so, then now is the time to set the address.
@ -1987,7 +2001,7 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv)
}
else
{
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
}
}
@ -1997,12 +2011,15 @@ static void stm32_ep0in(struct stm32_usbdev_s *priv)
static void stm32_ep0out(struct stm32_usbdev_s *priv)
{
int ret;
struct stm32_ep_s *privep = &priv->eplist[EP0];
switch (priv->devstate)
switch (priv->ep0state)
{
case DEVSTATE_RDREQUEST: /* Write request in progress */
case DEVSTATE_IDLE: /* No transfer in progress */
stm32_rdrequest(priv, privep);
case EP0STATE_RDREQUEST: /* Write request in progress */
case EP0STATE_IDLE: /* No transfer in progress */
ret = stm32_rdrequest(priv, privep);
priv->ep0state = ((ret == OK) ? EP0STATE_RDREQUEST : EP0STATE_IDLE);
break;
default:
@ -2010,7 +2027,7 @@ static void stm32_ep0out(struct stm32_usbdev_s *priv)
* completed, STALL the endpoint in either case
*/
priv->devstate = DEVSTATE_STALLED;
priv->ep0state = EP0STATE_STALLED;
break;
}
}
@ -2103,7 +2120,7 @@ static inline void stm32_ep0done(struct stm32_usbdev_s *priv, uint16_t istr)
/* Now figure out the new RX/TX status. Here are all possible
* consequences of the above EP0 operations:
*
* rxstatus txstatus devstate MEANING
* rxstatus txstatus ep0state MEANING
* -------- -------- --------- ---------------------------------
* NAK NAK IDLE Nothing happened
* NAK VALID IDLE EP0 response sent from USBDEV driver
@ -2113,9 +2130,9 @@ static inline void stm32_ep0done(struct stm32_usbdev_s *priv, uint16_t istr)
* First handle the STALL condition:
*/
if (priv->devstate == DEVSTATE_STALLED)
if (priv->ep0state == EP0STATE_STALLED)
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EP0SETUPSTALLED), priv->devstate);
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EP0SETUPSTALLED), priv->ep0state);
priv->rxstatus = USB_EPR_STATRX_STALL;
priv->txstatus = USB_EPR_STATTX_STALL;
}
@ -2843,7 +2860,7 @@ static int stm32_epsubmit(struct usbdev_ep_s *ep, struct usbdev_req_s *req)
if (!privep->txbusy)
{
priv->txstatus = USB_EPR_STATTX_NAK;
ret = stm32_wrrequest(priv, privep);
ret = stm32_wrrequest(priv, privep);
/* Set the new TX status */
@ -2955,7 +2972,12 @@ static int stm32_epstall(struct usbdev_ep_s *ep, bool resume)
if (status == 0)
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_EPDISABLED), 0);
priv->devstate = DEVSTATE_STALLED;
if (epno == 0)
{
priv->ep0state = EP0STATE_STALLED;
}
return -ENODEV;
}
@ -3256,7 +3278,7 @@ static void stm32_reset(struct stm32_usbdev_s *priv)
/* Reset the device state structure */
priv->devstate = DEVSTATE_IDLE;
priv->ep0state = EP0STATE_IDLE;
priv->rsmstate = RSMSTATE_IDLE;
priv->rxpending = false;
@ -3466,27 +3488,48 @@ void up_usbinitialize(void)
usbtrace(TRACE_DEVINIT, 0);
stm32_checksetup();
/* Configure USB GPIO alternate function pins */
#ifdef CONFIG_STM32_STM32F30XX
(void)stm32_configgpio(GPIO_USB_DM);
(void)stm32_configgpio(GPIO_USB_DP);
#endif
/* Power up the USB controller, but leave it in the reset state */
stm32_hwsetup(priv);
/* Remap the USB interrupt as needed (Only supported by the STM32 F3 family) */
#ifdef CONFIG_STM32_STM32F30XX
# ifdef CONFIG_STM32_USB_ITRMP
/* Clear the ITRMP bit to use the legacy, shared USB/CAN interrupts */
modifyreg32(STM32_RCC_APB1ENR, SYSCFG_CFGR1_USB_ITRMP, 0);
# else
/* Set the ITRMP bit to use the STM32 F3's dedicated USB interrupts */
modifyreg32(STM32_RCC_APB1ENR, 0, SYSCFG_CFGR1_USB_ITRMP);
# endif
#endif
/* Attach USB controller interrupt handlers. The hardware will not be
* initialized and interrupts will not be enabled until the class device
* driver is bound. Getting the IRQs here only makes sure that we have
* them when we need them later.
*/
if (irq_attach(STM32_IRQ_USBHPCANTX, stm32_hpinterrupt) != 0)
if (irq_attach(STM32_IRQ_USBHP, stm32_hpinterrupt) != 0)
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_IRQREGISTRATION),
(uint16_t)STM32_IRQ_USBHPCANTX);
(uint16_t)STM32_IRQ_USBHP);
goto errout;
}
if (irq_attach(STM32_IRQ_USBLPCANRX0, stm32_lpinterrupt) != 0)
if (irq_attach(STM32_IRQ_USBLP, stm32_lpinterrupt) != 0)
{
usbtrace(TRACE_DEVERROR(STM32_TRACEERR_IRQREGISTRATION),
(uint16_t)STM32_IRQ_USBLPCANRX0);
(uint16_t)STM32_IRQ_USBLP);
goto errout;
}
return;
@ -3522,10 +3565,10 @@ void up_usbuninitialize(void)
/* Disable and detach the USB IRQs */
up_disable_irq(STM32_IRQ_USBHPCANTX);
up_disable_irq(STM32_IRQ_USBLPCANRX0);
irq_detach(STM32_IRQ_USBHPCANTX);
irq_detach(STM32_IRQ_USBLPCANRX0);
up_disable_irq(STM32_IRQ_USBHP);
up_disable_irq(STM32_IRQ_USBLP);
irq_detach(STM32_IRQ_USBHP);
irq_detach(STM32_IRQ_USBLP);
if (priv->driver)
{
@ -3595,13 +3638,13 @@ int usbdev_register(struct usbdevclass_driver_s *driver)
/* Enable USB controller interrupts at the NVIC */
up_enable_irq(STM32_IRQ_USBHPCANTX);
up_enable_irq(STM32_IRQ_USBLPCANRX0);
up_enable_irq(STM32_IRQ_USBHP);
up_enable_irq(STM32_IRQ_USBLP);
/* Set the interrrupt priority */
up_prioritize_irq(STM32_IRQ_USBHPCANTX, CONFIG_USB_PRI);
up_prioritize_irq(STM32_IRQ_USBLPCANRX0, CONFIG_USB_PRI);
up_prioritize_irq(STM32_IRQ_USBHP, CONFIG_USB_PRI);
up_prioritize_irq(STM32_IRQ_USBLP, CONFIG_USB_PRI);
/* Enable pull-up to connect the device. The host should enumerate us
* some time after this
@ -3657,8 +3700,8 @@ int usbdev_unregister(struct usbdevclass_driver_s *driver)
/* Disable USB controller interrupts (but keep them attached) */
up_disable_irq(STM32_IRQ_USBHPCANTX);
up_disable_irq(STM32_IRQ_USBLPCANRX0);
up_disable_irq(STM32_IRQ_USBHP);
up_disable_irq(STM32_IRQ_USBLP);
/* Put the hardware in an inactive state. Then bring the hardware back up
* in the reset state (this is probably not necessary, the stm32_reset()

2
nuttx/configs/px4fmu/include/board.h
View File

@ -299,7 +299,7 @@
#define PX4_I2C_OBDEV_EEPROM NOTDEFINED
#define PX4_I2C_OBDEV_PX4IO_BL 0x18
#define PX4_I2C_OBDEV_PX4IO 0x19
#define PX4_I2C_OBDEV_PX4IO 0x1a
/*
* SPI

13
nuttx/configs/px4fmu/nsh/appconfig
View File

@ -46,14 +46,18 @@ CONFIGURED_APPS += systemlib
CONFIGURED_APPS += systemlib/mixer
# Math library
ifneq ($(CONFIG_APM),y)
CONFIGURED_APPS += mathlib
CONFIGURED_APPS += mathlib/CMSIS
CONFIGURED_APPS += examples/math_demo
endif
# Control library
ifneq ($(CONFIG_APM),y)
CONFIGURED_APPS += controllib
CONFIGURED_APPS += examples/control_demo
CONFIGURED_APPS += examples/kalman_demo
endif
# System utility commands
CONFIGURED_APPS += systemcmds/reboot
@ -63,6 +67,7 @@ CONFIGURED_APPS += systemcmds/boardinfo
CONFIGURED_APPS += systemcmds/mixer
CONFIGURED_APPS += systemcmds/eeprom
CONFIGURED_APPS += systemcmds/param
CONFIGURED_APPS += systemcmds/pwm
CONFIGURED_APPS += systemcmds/bl_update
CONFIGURED_APPS += systemcmds/preflight_check
CONFIGURED_APPS += systemcmds/delay_test
@ -77,7 +82,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
@ -87,6 +92,8 @@ CONFIGURED_APPS += mavlink_onboard
CONFIGURED_APPS += commander
CONFIGURED_APPS += sdlog
CONFIGURED_APPS += sensors
ifneq ($(CONFIG_APM),y)
CONFIGURED_APPS += ardrone_interface
CONFIGURED_APPS += multirotor_att_control
CONFIGURED_APPS += multirotor_pos_control
@ -96,10 +103,11 @@ CONFIGURED_APPS += fixedwing_pos_control
CONFIGURED_APPS += position_estimator
CONFIGURED_APPS += attitude_estimator_ekf
CONFIGURED_APPS += hott_telemetry
endif
# Hacking tools
#CONFIGURED_APPS += system/i2c
#CONFIGURED_APPS += tools/i2c_dev
CONFIGURED_APPS += systemcmds/i2c
# Communication and Drivers
CONFIGURED_APPS += drivers/boards/px4fmu
@ -118,6 +126,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

6
nuttx/configs/px4fmu/nsh/defconfig
View File

@ -369,11 +369,12 @@ CONFIG_I2C_WRITEREAD=y
# I2C configuration
#
CONFIG_I2C=y
CONFIG_I2C_POLLED=y
CONFIG_I2C_POLLED=n
CONFIG_I2C_TRANSFER=y
CONFIG_I2C_TRACE=n
CONFIG_I2C_RESET=y
# XXX fixed per-transaction timeout
CONFIG_STM32_I2CTIMEOMS=10
# XXX re-enable after integration testing
@ -778,6 +779,7 @@ CONFIG_NXFFS_MAXNAMLEN=32
CONFIG_NXFFS_TAILTHRESHOLD=2048
CONFIG_NXFFS_PREALLOCATED=y
CONFIG_FS_ROMFS=y
CONFIG_FS_BINFS=y
#
# SPI-based MMC/SD driver

20
nuttx/configs/px4io/io/defconfig
View File

@ -86,7 +86,7 @@ CONFIG_ARCH_BOOTLOADER=n
CONFIG_ARCH_LEDS=n
CONFIG_ARCH_BUTTONS=n
CONFIG_ARCH_CALIBRATION=n
CONFIG_ARCH_DMA=n
CONFIG_ARCH_DMA=y
CONFIG_ARCH_MATH_H=y
CONFIG_ARMV7M_CMNVECTOR=y
@ -112,7 +112,7 @@ CONFIG_STM32_JTAG_SW_ENABLE=n
# Individual subsystems can be enabled:
#
# AHB:
CONFIG_STM32_DMA1=n
CONFIG_STM32_DMA1=y
CONFIG_STM32_DMA2=n
CONFIG_STM32_CRC=n
# APB1:
@ -189,6 +189,12 @@ CONFIG_USART1_2STOP=0
CONFIG_USART2_2STOP=0
CONFIG_USART3_2STOP=0
CONFIG_USART1_RXDMA=y
SERIAL_HAVE_CONSOLE_DMA=y
# Conflicts with I2C1 DMA
CONFIG_USART2_RXDMA=n
CONFIG_USART3_RXDMA=y
#
# PX4IO specific driver settings
#
@ -399,7 +405,7 @@ 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
@ -469,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
@ -535,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=

52
nuttx/libc/stdio/lib_sscanf.c
View File

@ -197,7 +197,7 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap)
noassign = false;
lflag = false;
while (*fmt && *buf)
while (*fmt)
{
/* Skip over white space */
@ -242,6 +242,33 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap)
fmt--;
}
}
/* Process %n: Character count */
if (*fmt == 'n')
{
lvdbg("vsscanf: Performing character count\n");
if (!noassign)
{
size_t nchars = (size_t)(buf - bufstart);
if (lflag)
{
long *plong = va_arg(ap, long*);
*plong = (long)nchars;
}
else
{
int *pint = va_arg(ap, int*);
*pint = (int)nchars;
}
}
} else {
/* Check for valid data in input string */
if (!(*buf))
break;
/* Process %s: String conversion */
@ -445,28 +472,7 @@ int vsscanf(FAR char *buf, FAR const char *fmt, va_list ap)
#endif
}
/* Process %n: Character count */
else if (*fmt == 'n')
{
lvdbg("vsscanf: Performing character count\n");
if (!noassign)
{
size_t nchars = (size_t)(buf - bufstart);
if (lflag)
{
long *plong = va_arg(ap, long*);
*plong = (long)nchars;
}
else
{
int *pint = va_arg(ap, int*);
*pint = (int)nchars;
}
}
}
}
/* Note %n does not count as a conversion */