Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 14:40:12 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

Merge branch 'master' into export-build

Browse Source
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
Download Patch File Download Diff File Expand all files Collapse all files
Debug/ARMv7M
+164
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
Debug/NuttX
+4
View File
@@ -174,12 +174,16 @@ end
define showtaskstack
set $task = (struct _TCB *)$arg0
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
end
#
# Print details of a task
Debug/PX4
+2 -1
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
Firmware.sublime-project
+1
View File
@@ -7,6 +7,7 @@
[
"*.o",
"*.a",
"*.d",
".built",
".context",
".depend",
ROMFS/px4fmu_default/logging/logconv.m
+2 -2
View File
@@ -33,7 +33,7 @@ end
% float vbat; //battery voltage in [volt]
% float bat_current - current drawn from battery at this time instant
% float bat_discharged - discharged energy in mAh
% float adc[3]; //remaining auxiliary ADC ports [volt]
% float adc[4]; //ADC ports [volt]
% float local_position[3]; //tangent plane mapping into x,y,z [m]
% int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter]
% float attitude[3]; //pitch, roll, yaw [rad]
@@ -57,7 +57,7 @@ logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, '
logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le');
logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le');
ROMFS/px4fmu_default/mixers/FMU_Q.mix
+2 -2
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
--------
ROMFS/px4fmu_default/mixers/FMU_X5.mix
+4 -4
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
--------
Tools/px_mkfw.py
+1 -1
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))
Tools/px_uploader.py
+8 -8
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
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+33 -33
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);
apps/commander/commander.c
+132 -6
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;
}
apps/controllib/block/BlockParam.cpp
+4 -2
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);
apps/controllib/block/BlockParam.hpp
+8 -3
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();
}
apps/controllib/fixedwing.cpp
+7 -7
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;
}
apps/drivers/blinkm/blinkm.cpp
+5 -5
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() {
apps/drivers/bma180/bma180.cpp
+1 -1
View File
@@ -126,7 +126,7 @@ class BMA180 : public device::SPI
{
public:
BMA180(int bus, spi_dev_e device);
~BMA180();
virtual ~BMA180();
virtual int init();
apps/drivers/boards/px4fmu/px4fmu_spi.c
+7 -7
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)
apps/drivers/boards/px4io/px4io_internal.h
+2 -2
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 ********************************************************************/
apps/drivers/device/i2c.cpp
+38 -3
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
/* 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
apps/drivers/device/i2c.h
+10
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.
*
apps/drivers/drv_hrt.h
+16
View File
@@ -91,6 +91,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts);
*/
__EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime);
/*
* Compute the delta between a timestamp taken in the past
* and now.
*
* This function is safe to use even if the timestamp is updated
* by an interrupt during execution.
*/
__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then);
/*
* Store the absolute time in an interrupt-safe fashion.
*
* This function ensures that the timestamp cannot be seen half-written by an interrupt handler.
*/
__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now);
/*
* Call callout(arg) after delay has elapsed.
*
apps/drivers/drv_pwm_output.h
+35 -9
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,15 +100,25 @@ 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 */
/** 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)
/** 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.
*
apps/drivers/drv_range_finder.h
+81
View File
@@ -0,0 +1,81 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file Rangefinder driver interface.
*/
#ifndef _DRV_RANGEFINDER_H
#define _DRV_RANGEFINDER_H
#include <stdint.h>
#include <sys/ioctl.h>
#include "drv_sensor.h"
#include "drv_orb_dev.h"
#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder"
/**
* range finder report structure. Reads from the device must be in multiples of this
* structure.
*/
struct range_finder_report {
uint64_t timestamp;
float distance; /** in meters */
uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */
};
/*
* ObjDev tag for raw range finder data.
*/
ORB_DECLARE(sensor_range_finder);
/*
* ioctl() definitions
*
* Rangefinder drivers also implement the generic sensor driver
* interfaces from drv_sensor.h
*/
#define _RANGEFINDERIOCBASE (0x7900)
#define __RANGEFINDERIOC(_n) (_IOC(_RANGEFINDERIOCBASE, _n))
/** set the minimum effective distance of the device */
#define RANGEFINDERIOCSETMINIUMDISTANCE __RANGEFINDERIOC(1)
/** set the maximum effective distance of the device */
#define RANGEFINDERIOCSETMAXIUMDISTANCE __RANGEFINDERIOC(2)
#endif /* _DRV_RANGEFINDER_H */
apps/drivers/drv_rc_input.h
+7
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 */
apps/drivers/gps/gps.cpp
+2 -2
View File
@@ -67,7 +67,7 @@
#include "mtk.h"
#define TIMEOUT_5HZ 400
#define TIMEOUT_5HZ 500
#define RATE_MEASUREMENT_PERIOD 5000000
/* oddly, ERROR is not defined for c++ */
@@ -86,7 +86,7 @@ class GPS : public device::CDev
{
public:
GPS(const char* uart_path);
~GPS();
virtual ~GPS();
virtual int init();
apps/drivers/hil/hil.cpp
+33 -41
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,60 +798,35 @@ 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")) {
// 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")) {
} else if (!strcmp(verb, "mode_pwm_serial")) {
new_mode = PORT1_PWM_AND_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
} else if (!strcmp(verb, "mode_pwm_gpio")) {
new_mode = PORT1_PWM_AND_GPIO;
} else if (!strcmp(argv[i], "mode_port2_pwm8")) {
} else if (!strcmp(verb, "mode_port2_pwm8")) {
new_mode = PORT2_8PWM;
} else if (!strcmp(argv[i], "mode_port2_pwm12")) {
} else if (!strcmp(verb, "mode_port2_pwm12")) {
new_mode = PORT2_8PWM;
} else if (!strcmp(argv[i], "mode_port2_pwm16")) {
} else if (!strcmp(verb, "mode_port2_pwm16")) {
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");
// }
}
}
/* 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;
}
apps/drivers/hmc5883/hmc5883.cpp
+2 -2
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) ||
apps/drivers/l3gd20/l3gd20.cpp
+1 -1
View File
@@ -152,7 +152,7 @@ class L3GD20 : public device::SPI
{
public:
L3GD20(int bus, const char* path, spi_dev_e device);
~L3GD20();
virtual ~L3GD20();
virtual int init();
apps/drivers/led/led.cpp
+1 -1
View File
@@ -53,7 +53,7 @@ class LED : device::CDev
{
public:
LED();
~LED();
virtual ~LED();
virtual int init();
virtual int ioctl(struct file *filp, int cmd, unsigned long arg);
apps/drivers/mb12xx/Makefile
+42
View File
@@ -0,0 +1,42 @@
############################################################################
#
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Makefile to build the Maxbotix Sonar driver.
#
APPNAME = mb12xx
PRIORITY = SCHED_PRIORITY_DEFAULT
STACKSIZE = 2048
include $(APPDIR)/mk/app.mk
apps/drivers/mb12xx/mb12xx.cpp
+840
View File
File diff suppressed because it is too large Load Diff
apps/drivers/mpu6000/mpu6000.cpp
+1 -1
View File
@@ -151,7 +151,7 @@ class MPU6000 : public device::SPI
{
public:
MPU6000(int bus, spi_dev_e device);
~MPU6000();
virtual ~MPU6000();
virtual int init();
apps/drivers/ms5611/ms5611.cpp
+51 -19
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 */
apps/drivers/px4fmu/fmu.cpp
+189 -84
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);
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,59 +1033,33 @@ 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")) {
if (!strcmp(verb, "mode_gpio")) {
new_mode = PORT_FULL_GPIO;
} else if (!strcmp(argv[i], "mode_serial")) {
} else if (!strcmp(verb, "mode_serial")) {
new_mode = PORT_FULL_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm")) {
} else if (!strcmp(verb, "mode_pwm")) {
new_mode = PORT_FULL_PWM;
} else if (!strcmp(argv[i], "mode_gpio_serial")) {
} else if (!strcmp(verb, "mode_gpio_serial")) {
new_mode = PORT_GPIO_AND_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm_serial")) {
} else if (!strcmp(verb, "mode_pwm_serial")) {
new_mode = PORT_PWM_AND_SERIAL;
} else if (!strcmp(argv[i], "mode_pwm_gpio")) {
} else if (!strcmp(verb, "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");
}
}
}
/* was a new mode set? */
if (new_mode != PORT_MODE_UNSET) {
@@ -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);
}
apps/drivers/px4io/px4io.cpp
+1248 -478
View File
File diff suppressed because it is too large Load Diff
apps/drivers/px4io/uploader.h
+1 -1
View File
@@ -46,7 +46,7 @@ class PX4IO_Uploader
{
public:
PX4IO_Uploader();
~PX4IO_Uploader();
virtual ~PX4IO_Uploader();
int upload(const char *filenames[]);
apps/drivers/stm32/drv_hrt.c
+30
View File
@@ -645,6 +645,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime)
ts->tv_nsec = abstime * 1000;
}
/*
* Compare a time value with the current time.
*/
hrt_abstime
hrt_elapsed_time(const volatile hrt_abstime *then)
{
irqstate_t flags = irqsave();
hrt_abstime delta = hrt_absolute_time() - *then;
irqrestore(flags);
return delta;
}
/*
* Store the absolute time in an interrupt-safe fashion
*/
hrt_abstime
hrt_store_absolute_time(volatile hrt_abstime *now)
{
irqstate_t flags = irqsave();
hrt_abstime ts = hrt_absolute_time();
irqrestore(flags);
return ts;
}
/*
* Initalise the high-resolution timing module.
*/
apps/drivers/stm32/drv_pwm_servo.c
+40 -17
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;
}
}
}
apps/drivers/stm32/tone_alarm/tone_alarm.cpp
+669 -303
View File
File diff suppressed because it is too large Load Diff
apps/examples/control_demo/params.c
-4
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)
apps/examples/kalman_demo/KalmanNav.cpp
+2 -2
View File
@@ -247,8 +247,8 @@ void KalmanNav::update()
// output
if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz
_outTimeStamp = newTimeStamp;
printf("nav: %4d Hz, miss #: %4d\n",
_navFrames / 10, _miss / 10);
//printf("nav: %4d Hz, miss #: %4d\n",
// _navFrames / 10, _miss / 10);
_navFrames = 0;
_miss = 0;
}
apps/mavlink/mavlink_log.h
+12
View File
@@ -63,7 +63,11 @@
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
#else
#define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text);
#endif
/**
* Send a mavlink critical message.
@@ -71,7 +75,11 @@
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
#else
#define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text);
#endif
/**
* Send a mavlink info message.
@@ -79,7 +87,11 @@
* @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0);
* @param _text The text to log;
*/
#ifdef __cplusplus
#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
#else
#define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text);
#endif
struct mavlink_logmessage {
char text[51];
apps/mavlink/orb_listener.c
+48 -64
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;
apps/mavlink_onboard/mavlink.c
+1 -1
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);
apps/namedapp/namedapp_list.h
-42
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 },
apps/namedapp/namedapp_proto.h
-42
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[]);
apps/nshlib/Kconfig
+4
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
apps/nshlib/nsh.h
+3
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
apps/nshlib/nsh_fscmds.c
+81
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
apps/nshlib/nsh_parse.c
+3
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)

Some files were not shown because too many files have changed in this diff Show More