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

Merge branch 'fmuv2_bringup' of github.com:cvg/Firmware_Private into fmuv2_bringup

Browse Source
This commit is contained in:
Lorenz Meier
2013-07-26 15:16:58 +02:00
parent dd3033fa6f ffd14e1396
commit 18635c5f5f
6 changed files with 104 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/10_io_f330
+7 -8
View File
@@ -15,20 +15,19 @@ then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
param set MC_ATTRATE_D 0.007
param set MC_ATTRATE_D 0.005
param set MC_ATTRATE_I 0.0
param set MC_ATTRATE_P 0.13
param set MC_ATTRATE_P 0.1
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
param set MC_ATT_P 7.0
param set MC_POS_P 0.1
param set MC_ATT_P 4.5
param set MC_RCLOSS_THR 0.0
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.5
param set MC_YAWPOS_P 1.0
param set MC_YAWPOS_I 0.3
param set MC_YAWPOS_P 0.6
param set MC_YAWRATE_D 0.0
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_P 0.1
param save /fs/microsd/params
fi
@@ -128,7 +127,7 @@ multirotor_att_control start
#
# Start logging
#
sdlog2 start -r 20 -a -b 14
sdlog2 start -r 20 -a -b 16
#
# Start system state
src/drivers/boards/px4fmuv2/px4fmu2_init.c
+2 -2
View File
@@ -193,8 +193,8 @@ __EXPORT int nsh_archinitialize(void)
NULL);
/* initial LED state */
//drv_led_start();
up_ledoff(LED_AMBER);
drv_led_start();
led_off(LED_AMBER);
/* Configure SPI-based devices */
src/drivers/boards/px4fmuv2/px4fmu2_led.c
+2 -2
View File
@@ -68,7 +68,7 @@ __EXPORT void led_init()
__EXPORT void led_on(int led)
{
if (led == 0)
if (led == 1)
{
/* Pull down to switch on */
stm32_gpiowrite(GPIO_LED1, false);
@@ -77,7 +77,7 @@ __EXPORT void led_on(int led)
__EXPORT void led_off(int led)
{
if (led == 0)
if (led == 1)
{
/* Pull up to switch off */
stm32_gpiowrite(GPIO_LED1, true);
src/drivers/ms5611/ms5611.cpp
+90 -58
View File
@@ -111,9 +111,8 @@ protected:
ms5611::prom_s _prom;
struct hrt_call _baro_call;
unsigned _call_baro_interval;
struct work_s _work;
unsigned _measure_ticks;
unsigned _num_reports;
volatile unsigned _next_report;
@@ -144,12 +143,12 @@ protected:
* @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
@@ -167,11 +166,12 @@ protected:
void cycle();
/**
* Static trampoline; XXX is this needed?
* Static trampoline from the workq context; because we don't have a
* generic workq wrapper yet.
*
* @param arg Instance pointer for the driver that is polling.
*/
void cycle_trampoline(void *arg);
static void cycle_trampoline(void *arg);
/**
* Issue a measurement command for the current state.
@@ -184,7 +184,6 @@ protected:
* Collect the result of the most recent measurement.
*/
virtual int collect();
};
/*
@@ -196,7 +195,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
CDev("MS5611", BARO_DEVICE_PATH),
_interface(interface),
_prom(prom_buf.s),
_call_baro_interval(0),
_measure_ticks(0),
_num_reports(0),
_next_report(0),
_oldest_report(0),
@@ -213,13 +212,14 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) :
_comms_errors(perf_alloc(PC_COUNT, "ms5611_comms_errors")),
_buffer_overflows(perf_alloc(PC_COUNT, "ms5611_buffer_overflows"))
{
// work_cancel in stop_cycle called from the dtor will explode if we don't do this...
memset(&_work, 0, sizeof(_work));
}
MS5611::~MS5611()
{
/* make sure we are truly inactive */
stop();
stop_cycle();
/* free any existing reports */
if (_reports != nullptr)
@@ -277,7 +277,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
return -ENOSPC;
/* if automatic measurement is enabled */
if (_call_baro_interval > 0) {
if (_measure_ticks > 0) {
/*
* While there is space in the caller's buffer, and reports, copy them.
@@ -298,13 +298,41 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
/* manual measurement - run one conversion */
/* XXX really it'd be nice to lock against other readers here */
_measure_phase = 0;
_oldest_report = _next_report = 0;
measure();
do {
_measure_phase = 0;
_oldest_report = _next_report = 0;
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
/* do temperature first */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* now do a pressure measurement */
if (OK != measure()) {
ret = -EIO;
break;
}
usleep(MS5611_CONVERSION_INTERVAL);
if (OK != collect()) {
ret = -EIO;
break;
}
/* state machine will have generated a report, copy it out */
memcpy(buffer, _reports, sizeof(*_reports));
ret = sizeof(*_reports);
} while (0);
return ret;
}
@@ -319,8 +347,8 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* switching to manual polling */
case SENSOR_POLLRATE_MANUAL:
stop();
_call_baro_interval = 0;
stop_cycle();
_measure_ticks = 0;
return OK;
/* external signalling not supported */
@@ -334,15 +362,14 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
case SENSOR_POLLRATE_MAX:
case SENSOR_POLLRATE_DEFAULT: {
/* do we need to start internal polling? */
bool want_start = (_call_baro_interval == 0);
/* set interval to minimum legal value */
_baro_call.period = _call_baro_interval = MS5611_CONVERSION_INTERVAL;
bool want_start = (_measure_ticks == 0);
/* set interval for next measurement to minimum legal value */
_measure_ticks = USEC2TICK(MS5611_CONVERSION_INTERVAL);
/* if we need to start the poll state machine, do it */
if (want_start)
start();
start_cycle();
return OK;
}
@@ -350,18 +377,21 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
/* adjust to a legal polling interval in Hz */
default: {
/* do we need to start internal polling? */
bool want_start = (_call_baro_interval == 0);
bool want_start = (_measure_ticks == 0);
/* convert hz to tick interval via microseconds */
unsigned ticks = USEC2TICK(1000000 / arg);
/* check against maximum rate */
if (1000000/arg < MS5611_CONVERSION_INTERVAL)
if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
return -EINVAL;
/* update interval */
_baro_call.period = _call_baro_interval = 1000000/arg;
/* update interval for next measurement */
_measure_ticks = ticks;
/* if we need to start the poll state machine, do it */
if (want_start)
start();
start_cycle();
return OK;
}
@@ -369,10 +399,10 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
}
case SENSORIOCGPOLLRATE:
if (_call_baro_interval == 0)
if (_measure_ticks == 0)
return SENSOR_POLLRATE_MANUAL;
return (1000 / _call_baro_interval);
return (1000 / _measure_ticks);
case SENSORIOCSQUEUEDEPTH: {
/* add one to account for the sentinel in the ring */
@@ -389,11 +419,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;
}
@@ -427,32 +457,30 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
}
void
MS5611::start()
MS5611::start_cycle()
{
/* make sure we are stopped first */
stop();
/* reset the report ring */
_oldest_report = _next_report = 0;
/* reset to first measure phase */
/* reset the report ring and state machine */
_collect_phase = false;
_measure_phase = 0;
_oldest_report = _next_report = 0;
/* schedule a cycle to start things */
hrt_call_every(&_baro_call, 1000, _call_baro_interval, (hrt_callout)&MS5611::cycle_trampoline, this);
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
}
void
MS5611::stop()
MS5611::stop_cycle()
{
hrt_cancel(&_baro_call);
work_cancel(HPWORK, &_work);
}
void
MS5611::cycle_trampoline(void *arg)
{
MS5611 *dev = (MS5611 *)arg;
MS5611 *dev = reinterpret_cast<MS5611 *>(arg);
cycle();
dev->cycle();
}
void
@@ -476,7 +504,7 @@ MS5611::cycle()
//log("collection error %d", ret);
}
/* reset the collection state machine and try again */
start();
start_cycle();
return;
}
@@ -489,16 +517,14 @@ MS5611::cycle()
* doing pressure measurements at something close to the desired rate.
*/
if ((_measure_phase != 0) &&
(_call_baro_interval > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
(_measure_ticks > USEC2TICK(MS5611_CONVERSION_INTERVAL))) {
// XXX maybe do something appropriate as well
// /* schedule a fresh cycle call when we are ready to measure again */
// work_queue(HPWORK,
// &_work,
// (worker_t)&MS5611::cycle_trampoline,
// this,
// _measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
/* schedule a fresh cycle call when we are ready to measure again */
work_queue(HPWORK,
&_work,
(worker_t)&MS5611::cycle_trampoline,
this,
_measure_ticks - USEC2TICK(MS5611_CONVERSION_INTERVAL));
return;
}
@@ -509,12 +535,19 @@ MS5611::cycle()
if (ret != OK) {
//log("measure error %d", ret);
/* reset the collection state machine and try again */
start();
start_cycle();
return;
}
/* next phase is collection */
_collect_phase = true;
/* schedule a fresh cycle call when the measurement is done */
work_queue(HPWORK,
&_work,
(worker_t)&MS5611::cycle_trampoline,
this,
USEC2TICK(MS5611_CONVERSION_INTERVAL));
}
int
@@ -663,14 +696,13 @@ MS5611::collect()
return OK;
}
void
MS5611::print_info()
{
perf_print_counter(_sample_perf);
perf_print_counter(_comms_errors);
perf_print_counter(_buffer_overflows);
printf("poll interval: %u ticks\n", _call_baro_interval);
printf("poll interval: %u ticks\n", _measure_ticks);
printf("report queue: %u (%u/%u @ %p)\n",
_num_reports, _oldest_report, _next_report, _reports);
printf("TEMP: %d\n", _TEMP);
src/modules/commander/accelerometer_calibration.c
+2 -2
View File
@@ -268,8 +268,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
/* EMA time constant in seconds*/
float ema_len = 0.2f;
/* set "still" threshold to 0.1 m/s^2 */
float still_thr2 = pow(0.1f, 2);
/* set "still" threshold to 0.25 m/s^2 */
float still_thr2 = pow(0.25f, 2);
/* set accel error threshold to 5m/s^2 */
float accel_err_thr = 5.0f;
/* still time required in us */
src/modules/sensors/sensors.cpp
+1 -1
View File
@@ -391,7 +391,7 @@ namespace sensors
#endif
static const int ERROR = -1;
Sensors *g_sensors;
Sensors *g_sensors = nullptr;
}
Sensors::Sensors() :