fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 18:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into inav_fix

Browse Source
This commit is contained in:
Anton Babushkin
2013-09-27 18:29:16 +02:00
parent 4124417934 642081ddfe
commit 45a95ab5e7
29 changed files with 312 additions and 158 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/100_mpx_easystar
+14 -9
View File
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,12 +38,14 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
commander start
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
#
# Start the sensors and test them.
#
@@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
@@ -71,7 +69,14 @@ att_pos_estimator_ekf start
#
# Load mixer and start controllers (depends on px4io)
#
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
if [ -f /fs/microsd/etc/mixers/FMU_RET.mix ]
then
echo "Using FMU_RET mixer from sd card"
mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_RET.mix
else
echo "Using standard FMU_RET mixer"
mixer load /dev/pwm_output /etc/mixers/FMU_RET.mix
fi
fw_att_control start
# Not ready yet for prime-time
#fw_pos_control_l1 start
ROMFS/px4fmu_common/init.d/101_hk_bixler
+5 -7
View File
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,7 +38,9 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
commander start
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
@@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
ROMFS/px4fmu_common/init.d/30_io_camflyer
+5 -7
View File
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,7 +38,9 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
commander start
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
@@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
ROMFS/px4fmu_common/init.d/31_io_phantom
+5 -7
View File
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
usleep 5000
commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,7 +38,9 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
usleep 5000
commander start
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
@@ -53,11 +56,6 @@ sh /etc/init.d/rc.sensors
#
sh /etc/init.d/rc.logging
#
# Start the commander.
#
commander start
#
# Start GPS interface (depends on orb)
#
makefiles/config_px4fmu-v1_default.mk
+1
View File
@@ -63,6 +63,7 @@ MODULES += systemcmds/nshterm
# General system control
#
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
MODULES += modules/gpio_led
makefiles/config_px4fmu-v2_default.mk
+2
View File
@@ -51,6 +51,7 @@ MODULES += systemcmds/param
MODULES += systemcmds/perf
MODULES += systemcmds/preflight_check
MODULES += systemcmds/pwm
MODULES += systemcmds/esc_calib
MODULES += systemcmds/reboot
MODULES += systemcmds/top
MODULES += systemcmds/tests
@@ -61,6 +62,7 @@ MODULES += systemcmds/nshterm
# General system control
#
MODULES += modules/commander
MODULES += modules/navigator
MODULES += modules/mavlink
MODULES += modules/mavlink_onboard
nuttx-configs/px4fmu-v1/nsh/defconfig
+1 -1
View File
@@ -262,7 +262,7 @@ CONFIG_STM32_USART=y
# U[S]ART Configuration
#
# CONFIG_USART1_RS485 is not set
CONFIG_USART1_RXDMA=y
# CONFIG_USART1_RXDMA is not set
# CONFIG_USART2_RS485 is not set
CONFIG_USART2_RXDMA=y
# CONFIG_USART3_RXDMA is not set
nuttx-configs/px4fmu-v2/nsh/defconfig
+1 -1
View File
@@ -410,7 +410,7 @@ CONFIG_START_DAY=1
CONFIG_DEV_CONSOLE=y
# CONFIG_MUTEX_TYPES is not set
CONFIG_PRIORITY_INHERITANCE=y
CONFIG_SEM_PREALLOCHOLDERS=8
CONFIG_SEM_PREALLOCHOLDERS=0
CONFIG_SEM_NNESTPRIO=8
# CONFIG_FDCLONE_DISABLE is not set
CONFIG_FDCLONE_STDIO=y
src/drivers/drv_gps.h
+1 -2
View File
@@ -51,8 +51,7 @@
typedef enum {
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
GPS_DRIVER_MODE_MTK,
GPS_DRIVER_MODE_NMEA,
GPS_DRIVER_MODE_MTK
} gps_driver_mode_t;
src/drivers/drv_tone_alarm.h
+1
View File
@@ -144,6 +144,7 @@ enum {
TONE_ARMING_WARNING_TUNE,
TONE_BATTERY_WARNING_SLOW_TUNE,
TONE_BATTERY_WARNING_FAST_TUNE,
TONE_GPS_WARNING_TUNE,
TONE_NUMBER_OF_TUNES
};
src/drivers/gps/gps.cpp
-15
View File
@@ -279,10 +279,6 @@ GPS::task_main()
_Helper = new MTK(_serial_fd, &_report);
break;
case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA
break;
default:
break;
}
@@ -328,10 +324,6 @@ GPS::task_main()
mode_str = "MTK";
break;
case GPS_DRIVER_MODE_NMEA:
mode_str = "NMEA";
break;
default:
break;
}
@@ -362,9 +354,6 @@ GPS::task_main()
_mode = GPS_DRIVER_MODE_UBX;
break;
// case GPS_DRIVER_MODE_NMEA:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
default:
break;
}
@@ -400,10 +389,6 @@ GPS::print_info()
warnx("protocol: MTK");
break;
case GPS_DRIVER_MODE_NMEA:
warnx("protocol: NMEA");
break;
default:
break;
}
src/drivers/gps/ubx.cpp
-1
View File
@@ -281,7 +281,6 @@ UBX::receive(unsigned timeout)
return 1;
} else {
warnx("ubx: timeout - no messages");
return -1;
}
src/drivers/px4fmu/fmu.cpp
+10 -7
View File
@@ -1096,10 +1096,11 @@ fmu_start(void)
void
test(void)
{
int fd;
int fd;
unsigned servo_count = 0;
unsigned pwm_value = 1000;
int direction = 1;
int ret;
fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
@@ -1114,9 +1115,9 @@ test(void)
warnx("Testing %u servos", (unsigned)servo_count);
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
@@ -1166,15 +1167,17 @@ test(void)
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
if (c == 0x03 || c == 0x63) {
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
break;
}
}
}
close(console);
close(fd);
exit(0);
src/drivers/px4io/px4io.cpp
+11 -7
View File
@@ -591,6 +591,9 @@ PX4IO::init()
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
/* get a status update from IO */
io_get_status();
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
log("INAIR RESTART RECOVERY (needs commander app running)");
@@ -1509,7 +1512,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
* even.
*/
unsigned total_len = sizeof(px4io_mixdata) + count;
if (total_len % 1) {
if (total_len % 2) {
msg->text[count] = '\0';
total_len++;
}
@@ -2128,10 +2131,9 @@ test(void)
if (ioctl(fd, PWM_SERVO_ARM, 0))
err(1, "failed to arm servos");
/* Open console directly to grab CTRL-C signal */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
warnx("Press CTRL-C or 'c' to abort.");
@@ -2172,10 +2174,12 @@ test(void)
/* Check if user wants to quit */
char c;
if (read(console, &c, 1) == 1) {
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("User abort\n");
close(console);
exit(0);
}
}
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+2
View File
@@ -333,6 +333,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_ARMING_WARNING_TUNE] = "MNT75L1O2G"; //arming warning
_default_tunes[TONE_BATTERY_WARNING_SLOW_TUNE] = "MBNT100a8"; //battery warning slow
_default_tunes[TONE_BATTERY_WARNING_FAST_TUNE] = "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8"; //battery warning fast
_default_tunes[TONE_GPS_WARNING_TUNE] = "MFT255L4AAAL1F#"; //gps warning slow
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -342,6 +343,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_ARMING_WARNING_TUNE] = "arming"; // arming warning
_tune_names[TONE_BATTERY_WARNING_SLOW_TUNE] = "slow_bat"; // battery warning slow
_tune_names[TONE_BATTERY_WARNING_FAST_TUNE] = "fast_bat"; // battery warning fast
_tune_names[TONE_GPS_WARNING_TUNE] = "gps_warning"; // gps warning
}
ToneAlarm::~ToneAlarm()
src/modules/commander/commander.cpp
+32 -12
View File
@@ -470,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM:
// XXX implement later
result = VEHICLE_CMD_RESULT_DENIED;
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
if (safety->safety_switch_available && !safety->safety_off) {
print_reject_arm("NOT ARMING: Press safety switch first.");
arming_res = TRANSITION_DENIED;
} else {
arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
}
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
}
}
}
break;
default:
@@ -806,16 +824,6 @@ int commander_thread_main(int argc, char *argv[])
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
orb_check(cmd_sub, &updated);
if (updated) {
/* got command */
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
handle_command(&status, &safety, &control_mode, &cmd, &armed);
}
/* update safety topic */
orb_check(safety_sub, &updated);
@@ -1147,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* handle commands last, as the system needs to be updated to handle them */
orb_check(cmd_sub, &updated);
if (updated) {
/* got command */
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
handle_command(&status, &safety, &control_mode, &cmd, &armed);
}
/* evaluate the navigation state machine */
transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
src/modules/mavlink/mavlink.c
+1 -1
View File
@@ -531,7 +531,7 @@ int mavlink_thread_main(int argc, char *argv[])
case 'b':
baudrate = strtoul(optarg, NULL, 10);
if (baudrate == 0)
if (baudrate < 9600 || baudrate > 921600)
errx(1, "invalid baud rate '%s'", optarg);
break;
src/modules/mavlink/mavlink_receiver.cpp
+37 -10
View File
@@ -102,6 +102,7 @@ struct vehicle_global_position_s hil_global_pos;
struct vehicle_attitude_s hil_attitude;
struct vehicle_gps_position_s hil_gps;
struct sensor_combined_s hil_sensors;
struct battery_status_s hil_battery_status;
static orb_advert_t pub_hil_global_pos = -1;
static orb_advert_t pub_hil_attitude = -1;
static orb_advert_t pub_hil_gps = -1;
@@ -111,6 +112,12 @@ static orb_advert_t pub_hil_accel = -1;
static orb_advert_t pub_hil_mag = -1;
static orb_advert_t pub_hil_baro = -1;
static orb_advert_t pub_hil_airspeed = -1;
static orb_advert_t pub_hil_battery = -1;
/* packet counter */
static int hil_counter = 0;
static int hil_frames = 0;
static uint64_t old_timestamp = 0;
static orb_advert_t cmd_pub = -1;
static orb_advert_t flow_pub = -1;
@@ -149,7 +156,8 @@ handle_message(mavlink_message_t *msg)
vcmd.param5 = cmd_mavlink.param5;
vcmd.param6 = cmd_mavlink.param6;
vcmd.param7 = cmd_mavlink.param7;
vcmd.command = cmd_mavlink.command;
// XXX do proper translation
vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command;
vcmd.target_system = cmd_mavlink.target_system;
vcmd.target_component = cmd_mavlink.target_component;
vcmd.source_system = msg->sysid;
@@ -207,7 +215,7 @@ handle_message(mavlink_message_t *msg)
vcmd.param5 = 0;
vcmd.param6 = 0;
vcmd.param7 = 0;
vcmd.command = MAV_CMD_DO_SET_MODE;
vcmd.command = VEHICLE_CMD_DO_SET_MODE;
vcmd.target_system = new_mode.target_system;
vcmd.target_component = MAV_COMP_ID_ALL;
vcmd.source_system = msg->sysid;
@@ -360,11 +368,6 @@ handle_message(mavlink_message_t *msg)
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
/* packet counter */
static uint16_t hil_counter = 0;
static uint16_t hil_frames = 0;
static uint64_t old_timestamp = 0;
/* sensors general */
hil_sensors.timestamp = hrt_absolute_time();
@@ -391,9 +394,9 @@ handle_message(mavlink_message_t *msg)
hil_sensors.accelerometer_range_m_s2 = 32.7f; // int16
/* adc */
hil_sensors.adc_voltage_v[0] = 0;
hil_sensors.adc_voltage_v[1] = 0;
hil_sensors.adc_voltage_v[2] = 0;
hil_sensors.adc_voltage_v[0] = 0.0f;
hil_sensors.adc_voltage_v[1] = 0.0f;
hil_sensors.adc_voltage_v[2] = 0.0f;
/* magnetometer */
float mga2ga = 1.0e-3f;
@@ -506,6 +509,18 @@ handle_message(mavlink_message_t *msg)
pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
}
/* fill in HIL battery status */
hil_battery_status.timestamp = hrt_absolute_time();
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.current_a = 10.0f;
/* lazily publish the battery voltage */
if (pub_hil_battery > 0) {
orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
} else {
pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
}
// increment counters
hil_counter++;
hil_frames++;
@@ -640,6 +655,18 @@ handle_message(mavlink_message_t *msg)
} else {
orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
}
/* fill in HIL battery status */
hil_battery_status.timestamp = hrt_absolute_time();
hil_battery_status.voltage_v = 11.1f;
hil_battery_status.current_a = 10.0f;
/* lazily publish the battery voltage */
if (pub_hil_battery > 0) {
orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status);
} else {
pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status);
}
}
if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
src/modules/mavlink/orb_topics.h
+1
View File
@@ -65,6 +65,7 @@
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <drivers/drv_rc_input.h>
struct mavlink_subscriptions {
src/modules/multirotor_pos_control/multirotor_pos_control.c
+5 -5
View File
@@ -199,7 +199,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
struct vehicle_local_position_setpoint_s local_pos_sp;
memset(&local_pos_sp, 0, sizeof(local_pos_sp));
struct vehicle_global_position_setpoint_s global_pos_sp;
memset(&global_pos_sp, 0, sizeof(local_pos_sp));
memset(&global_pos_sp, 0, sizeof(global_pos_sp));
struct vehicle_global_velocity_setpoint_s global_vel_sp;
memset(&global_vel_sp, 0, sizeof(global_vel_sp));
@@ -414,6 +414,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
}
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* local position setpoint is valid and can be used for auto loiter after position controlled mode */
@@ -436,7 +437,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = att.yaw;
mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
}
@@ -477,8 +477,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
} else {
local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
}
local_pos_sp.yaw = global_pos_sp.yaw;
att_sp.yaw_body = global_pos_sp.yaw;
mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
@@ -491,7 +489,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
local_pos_sp.x = local_pos.x;
local_pos_sp.y = local_pos.y;
local_pos_sp.yaw = att.yaw;
att_sp.yaw_body = global_pos_sp.yaw;
}
if (reset_auto_sp_z) {
@@ -515,6 +512,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
reset_mission_sp = true;
}
/* copy yaw setpoint to vehicle_local_position_setpoint topic */
local_pos_sp.yaw = att_sp.yaw_body;
/* reset setpoints after AUTO mode */
reset_man_sp_xy = true;
reset_man_sp_z = true;
src/modules/px4iofirmware/controls.c
+1 -1
View File
@@ -286,7 +286,7 @@ controls_tick() {
* requested override.
*
*/
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH))
if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
override = true;
if (override) {
src/modules/px4iofirmware/mixer.cpp
+1 -1
View File
@@ -221,7 +221,7 @@ mixer_tick(void)
}
/* do the calculations during the ramp for all at once */
if(esc_state == ESC_RAMP) {
if (esc_state == ESC_RAMP) {
ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
}
src/modules/systemlib/mixer/mixer.cpp
+16
View File
@@ -142,6 +142,22 @@ NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
{
NullMixer *nm = nullptr;
const char *end = buf + buflen;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0')
continue;
/* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending or space, so no split symbols / numbers. good. */
break;
} else {
return nm;
}
}
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
nm = new NullMixer;
src/modules/systemlib/mixer/mixer_multirotor.cpp
+17
View File
@@ -181,6 +181,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
char geomname[8];
int s[4];
int used;
const char *end = buf + buflen;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0')
continue;
/* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending or space, so no split symbols / numbers. good. */
break;
} else {
debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
return nullptr;
}
}
if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
debug("multirotor parse failed on '%s'", buf);
src/modules/systemlib/mixer/mixer_simple.cpp
+35 -9
View File
@@ -55,7 +55,7 @@
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
@@ -98,14 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
{
int ret;
int s[5];
int n = -1;
buf = findtag(buf, buflen, 'O');
if ((buf == nullptr) || (buflen < 12))
if ((buf == nullptr) || (buflen < 12)) {
debug("output parser failed finding tag, ret: '%s'", buf);
return -1;
}
if ((ret = sscanf(buf, "O: %d %d %d %d %d",
&s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
debug("scaler parse failed on '%s' (got %d)", buf, ret);
if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
&s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
return -1;
}
skipline(buf, buflen);
@@ -126,8 +129,10 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
int s[5];
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16))
if ((buf == nullptr) || (buflen < 16)) {
debug("contorl parser failed finding tag, ret: '%s'", buf);
return -1;
}
if (sscanf(buf, "S: %u %u %d %d %d %d %d",
&u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
@@ -156,6 +161,22 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
int used;
const char *end = buf + buflen;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0')
continue;
/* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending or space, so no split symbols / numbers. good. */
break;
} else {
debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
goto out;
}
}
/* get the base info for the mixer */
if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
debug("simple parse failed on '%s'", buf);
@@ -173,22 +194,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
mixinfo->control_count = inputs;
if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
goto out;
}
for (unsigned i = 0; i < inputs; i++) {
if (parse_control_scaler(end - buflen, buflen,
mixinfo->controls[i].scaler,
mixinfo->controls[i].control_group,
mixinfo->controls[i].control_index))
mixinfo->controls[i].control_index)) {
debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
goto out;
}
}
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
debug("loaded mixer with %d inputs", inputs);
debug("loaded mixer with %d input(s)", inputs);
} else {
debug("could not allocate memory for mixer");
src/modules/uORB/topics/vehicle_command.h
+1 -1
View File
@@ -120,7 +120,7 @@ struct vehicle_command_s
float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */
float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */
float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */
uint16_t command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */
enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */
uint8_t target_system; /**< System which should execute the command */
uint8_t target_component; /**< Component which should execute the command, 0 for all components */
uint8_t source_system; /**< System sending the command */
src/modules/uORB/topics/vehicle_gps_position.h
+3 -3
View File
@@ -56,9 +56,9 @@
struct vehicle_gps_position_s
{
uint64_t timestamp_position; /**< Timestamp for position information */
int32_t lat; /**< Latitude in 1E7 degrees */
int32_t lon; /**< Longitude in 1E7 degrees */
int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */
int32_t lat; /**< Latitude in 1E-7 degrees */
int32_t lon; /**< Longitude in 1E-7 degrees */
int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */
uint64_t timestamp_variance;
float s_variance_m_s; /**< speed accuracy estimate m/s */
src/systemcmds/esc_calib/esc_calib.c
+42 -49
View File
@@ -1,6 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
* Author: Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -45,6 +46,7 @@
#include <stdbool.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <sys/mount.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
@@ -63,7 +65,7 @@
static void usage(const char *reason);
__EXPORT int esc_calib_main(int argc, char *argv[]);
#define MAX_CHANNELS 8
#define MAX_CHANNELS 14
static void
usage(const char *reason)
@@ -82,22 +84,26 @@ usage(const char *reason)
int
esc_calib_main(int argc, char *argv[])
{
const char *dev = PWM_OUTPUT_DEVICE_PATH;
char *dev = PWM_OUTPUT_DEVICE_PATH;
char *ep;
bool channels_selected[MAX_CHANNELS] = {false};
int ch;
int ret;
char c;
struct pollfd fds;
fds.fd = 0; /* stdin */
fds.events = POLLIN;
if (argc < 2)
usage(NULL);
while ((ch = getopt(argc, argv, "d:")) != EOF) {
while ((ch = getopt(argc-1, argv, "d:")) != EOF) {
switch (ch) {
case 'd':
dev = optarg;
argc--;
argc-=2;
break;
default:
@@ -105,7 +111,7 @@ esc_calib_main(int argc, char *argv[])
}
}
if(argc < 1) {
if(argc < 2) {
usage("no channels provided");
}
@@ -123,35 +129,33 @@ esc_calib_main(int argc, char *argv[])
}
}
/* Wait for confirmation */
int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
if (!console)
err(1, "failed opening console");
warnx("ATTENTION, please remove or fix props before starting calibration!\n"
printf("\nATTENTION, please remove or fix propellers before starting calibration!\n"
"\n"
"Also press the arming switch first for safety off\n"
"Make sure\n"
"\t - that the ESCs are not powered\n"
"\t - that safety is off (two short blinks)\n"
"\t - that the controllers are stopped\n"
"\n"
"Do you really want to start calibration: y or n?\n");
"Do you want to start calibration now: y or n?\n");
/* wait for user input */
while (1) {
if (read(console, &c, 1) == 1) {
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 'y' || c == 'Y') {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("ESC calibration exited");
close(console);
printf("ESC calibration exited\n");
exit(0);
} else if (c == 'n' || c == 'N') {
warnx("ESC calibration aborted");
close(console);
printf("ESC calibration aborted\n");
exit(0);
} else {
warnx("Unknown input, ESC calibration aborted");
close(console);
printf("Unknown input, ESC calibration aborted\n");
exit(0);
}
}
@@ -163,23 +167,14 @@ esc_calib_main(int argc, char *argv[])
int fd = open(dev, 0);
if (fd < 0)
err(1, "can't open %s", dev);
// XXX arming not necessaire at the moment
// /* Then arm */
// ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
// if (ret != OK)
// err(1, "PWM_SERVO_SET_ARM_OK");
// ret = ioctl(fd, PWM_SERVO_ARM, 0);
// if (ret != OK)
// err(1, "PWM_SERVO_ARM");
/* Wait for user confirmation */
warnx("Set high PWM\n"
"Connect battery now and hit ENTER after the ESCs confirm the first calibration step");
printf("\nHigh PWM set\n"
"\n"
"Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
"\n");
fflush(stdout);
while (1) {
@@ -192,13 +187,15 @@ esc_calib_main(int argc, char *argv[])
}
}
if (read(console, &c, 1) == 1) {
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 13) {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("ESC calibration exited");
close(console);
exit(0);
}
}
@@ -209,7 +206,8 @@ esc_calib_main(int argc, char *argv[])
/* we don't need any more user input */
warnx("Set low PWM, hit ENTER when finished");
printf("Low PWM set, hit ENTER when finished\n"
"\n");
while (1) {
@@ -222,13 +220,15 @@ esc_calib_main(int argc, char *argv[])
}
}
if (read(console, &c, 1) == 1) {
ret = poll(&fds, 1, 0);
if (ret > 0) {
read(0, &c, 1);
if (c == 13) {
break;
} else if (c == 0x03 || c == 0x63 || c == 'q') {
warnx("ESC calibration exited");
close(console);
printf("ESC calibration exited\n");
exit(0);
}
}
@@ -237,14 +237,7 @@ esc_calib_main(int argc, char *argv[])
}
warnx("ESC calibration finished");
close(console);
// XXX disarming not necessaire at the moment
/* Now disarm again */
// ret = ioctl(fd, PWM_SERVO_DISARM, 0);
printf("ESC calibration finished\n");
exit(0);
}
}
src/systemcmds/tests/tests_file.c
+61 -2
View File
@@ -38,7 +38,9 @@
*/
#include <sys/stat.h>
#include <dirent.h>
#include <stdio.h>
#include <stddef.h>
#include <unistd.h>
#include <fcntl.h>
#include <systemlib/err.h>
@@ -52,7 +54,7 @@
int
test_file(int argc, char *argv[])
{
const iterations = 10;
const iterations = 200;
/* check if microSD card is mounted */
struct stat buffer;
@@ -63,15 +65,52 @@ test_file(int argc, char *argv[])
uint8_t buf[512];
hrt_abstime start, end;
perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes");
perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)");
int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
memset(buf, 0, sizeof(buf));
warnx("aligned write - please wait..");
if ((0x3 & (uintptr_t)buf))
warnx("memory is unaligned!");
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
perf_begin(wperf);
write(fd, buf, sizeof(buf));
fsync(fd);
perf_end(wperf);
}
end = hrt_absolute_time();
warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
perf_print_counter(wperf);
perf_free(wperf);
int ret = unlink("/fs/microsd/testfile");
if (ret)
err(1, "UNLINKING FILE FAILED");
warnx("unaligned write - please wait..");
struct {
uint8_t byte;
uint8_t unaligned[512];
} unaligned_buf;
if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0)
warnx("creating unaligned memory failed.");
wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)");
start = hrt_absolute_time();
for (unsigned i = 0; i < iterations; i++) {
perf_begin(wperf);
write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned));
fsync(fd);
perf_end(wperf);
}
end = hrt_absolute_time();
@@ -79,9 +118,29 @@ test_file(int argc, char *argv[])
close(fd);
warnx("%dKiB in %llu microseconds", iterations / 2, end - start);
perf_print_counter(wperf);
perf_free(wperf);
/* list directory */
DIR *d;
struct dirent *dir;
d = opendir("/fs/microsd");
if (d) {
while ((dir = readdir(d)) != NULL) {
//printf("%s\n", dir->d_name);
}
closedir(d);
warnx("directory listing ok (FS mounted and readable)");
} else {
/* failed opening dir */
err(1, "FAILED LISTING MICROSD ROOT DIRECTORY");
}
return 0;
}
#if 0