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

Various minor fixes and improvements across system

This commit is contained in:
Lorenz Meier 2012-09-05 18:05:11 +02:00
parent 84e11a0cac
commit db6ec2d7d2
7 changed files with 89 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
apps/ardrone_interface/ardrone_motor_control.c
View File

@ -43,6 +43,8 @@
#include <unistd.h>
#include <drivers/drv_gpio.h>
#include <arch/board/up_hrt.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_outputs.h>
#include <systemlib/err.h>
#include "ardrone_motor_control.h"
@ -270,29 +272,33 @@ int ar_init_motors(int ardrone_uart, int gpios)
usleep(UART_TRANSFER_TIME_BYTE_US*11);
ar_deselect_motor(gpios, i);
/* sleep 400 ms */
usleep(400000);
/* sleep 200 ms */
usleep(200000);
}
/* start the multicast part */
errcounter += ar_select_motor(gpios, 0);
/*
* first round
* write six times A0 - enable broadcast
* receive nothing
*/
// for (int i = 0; i < sizeof(multicastbuf); i++) {
write(ardrone_uart, multicastbuf, 6);
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
usleep(200000);
write(ardrone_uart, multicastbuf, 6);
/*
* second round
* write six times A0 - enable broadcast
* receive nothing
*/
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
/* set motors to zero speed */
/* set motors to zero speed (fsync is part of the write command */
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
fsync(ardrone_uart);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
@ -324,12 +330,27 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t
int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) {
const unsigned int min_motor_interval = 4900;
static uint64_t last_motor_time = 0;
static struct actuator_outputs_s outputs;
outputs.output[0] = motor1;
outputs.output[1] = motor2;
outputs.output[2] = motor3;
outputs.output[3] = motor4;
static orb_advert_t pub = 0;
if (pub == 0) {
pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs);
}
if (hrt_absolute_time() - last_motor_time > min_motor_interval) {
uint8_t buf[5] = {0};
ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
int ret;
ret = write(ardrone_fd, buf, sizeof(buf));
fsync(ardrone_fd);
/* publish just written values */
orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs);
if (ret == sizeof(buf)) {
return OK;
} else {

76
apps/commander/commander.c
View File

@ -973,7 +973,7 @@ int commander_thread_main(int argc, char *argv[])
uint8_t flight_env;
/* Initialize to 3.0V to make sure the low-pass loads below valid threshold */
float battery_voltage = VOLTAGE_BATTERY_HIGH_VOLTS;
float battery_voltage = 12.0f;
bool battery_voltage_valid = true;
bool low_battery_voltage_actions_done = false;
bool critical_battery_voltage_actions_done = false;
@ -1010,6 +1010,8 @@ int commander_thread_main(int argc, char *argv[])
/* now initialized */
commander_initialized = true;
uint64_t start_time = hrt_absolute_time();
while (!thread_should_exit) {
/* Get current values */
@ -1019,7 +1021,14 @@ int commander_thread_main(int argc, char *argv[])
battery_voltage = sensors.battery_voltage_v;
battery_voltage_valid = sensors.battery_voltage_valid;
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
/*
* Only update battery voltage estimate if voltage is
* valid and system has been running for half a second
*/
if (battery_voltage_valid && (hrt_absolute_time() - start_time > 500000)) {
bat_remain = battery_remaining_estimate_voltage(3, BAT_CHEM_LITHIUM_POLYMERE, battery_voltage);
}
/* Slow but important 8 Hz checks */
if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) {
@ -1084,6 +1093,38 @@ int commander_thread_main(int argc, char *argv[])
ioctl(buzzer, TONE_SET_ALARM, 0);
}
/* Check battery voltage */
/* write to sys_status */
current_status.voltage_battery = battery_voltage;
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
}
low_voltage_counter++;
}
/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
state_machine_emergency(stat_pub, &current_status, mavlink_fd);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
}
/* End battery voltage check */
/* Check if last transition deserved an audio event */
#warning This code depends on state that is no longer? maintained
#if 0
@ -1142,37 +1183,6 @@ int commander_thread_main(int argc, char *argv[])
//update_state_machine_got_position_fix(stat_pub, &current_status, mavlink_fd);
/* end: check gps */
/* Check battery voltage */
/* write to sys_status */
current_status.voltage_battery = battery_voltage;
/* if battery voltage is getting lower, warn using buzzer, etc. */
if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_LOW_VOLTS && false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
low_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] WARNING! LOW BATTERY!");
}
low_voltage_counter++;
}
/* Critical, this is rather an emergency, kill signal to sdlog and change state machine */
else if (battery_voltage_valid && (battery_voltage < VOLTAGE_BATTERY_CRITICAL_VOLTS && false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) {
if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
critical_battery_voltage_actions_done = true;
mavlink_log_critical(mavlink_fd, "[commander] EMERGENCY! CIRITICAL BATTERY!");
state_machine_emergency(stat_pub, &current_status, mavlink_fd);
}
critical_voltage_counter++;
} else {
low_voltage_counter = 0;
critical_voltage_counter = 0;
}
/* End battery voltage check */
/* Start RC state check */
bool prev_lost = current_status.rc_signal_lost;

2
apps/commander/commander.h
View File

@ -39,8 +39,6 @@
#ifndef COMMANDER_H_
#define COMMANDER_H_
#define VOLTAGE_BATTERY_HIGH_VOLTS 12.0f
#define VOLTAGE_BATTERY_LOW_VOLTS 10.5f
#define VOLTAGE_BATTERY_CRITICAL_VOLTS 10.0f
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f

6
apps/drivers/mpu6000/mpu6000.cpp
View File

@ -369,8 +369,10 @@ MPU6000::init()
write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
usleep(1000);
// FS & DLPF FS=2000 deg/s, DLPF = 98Hz (low pass filter)
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
// was 90 Hz, but this ruins quality and does not improve the
// system response
write_reg(MPUREG_CONFIG, BITS_DLPF_CFG_20HZ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);

19
apps/mavlink/mavlink.c
View File

@ -542,6 +542,7 @@ static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int ma
if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval);
if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval);
if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval);
if (subs->actuators_sub) orb_set_interval(subs->actuators_sub, min_interval);
break;
case MAVLINK_MSG_ID_MANUAL_CONTROL:
/* manual_control_setpoint triggers this message */
@ -712,7 +713,6 @@ static void *uorb_receiveloop(void *arg)
/* --- ACTUATOR CONTROL VALUE --- */
/* subscribe to ORB for actuator control */
subs->actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
orb_set_interval(subs->actuators_sub, 2000); /* 0.5 Hz updates */
fds[fdsc_count].fd = subs->actuators_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
@ -1518,23 +1518,24 @@ int mavlink_thread_main(int argc, char *argv[])
/* all subscriptions are now active, set up initial guess about rate limits */
if (baudrate >= 921600) {
/* 500 Hz / 2 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 2);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
/* 5 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200);
} else if (baudrate >= 460800) {
/* 250 Hz / 4 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5);
/* 200 Hz / 5 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 5);
/* 100 Hz / 10 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
/* 50 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 10);
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 10);
/* 66 Hz / 15 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 15);
/* 20 Hz / 20 ms */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
/* 2 Hz */
set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
} else if (baudrate >= 115200) {

1
apps/multirotor_att_control/multirotor_att_control_main.c
View File

@ -144,6 +144,7 @@ mc_thread_main(int argc, char *argv[])
att_sp.roll_body = -manual.roll * M_PI_F / 8.0f;
att_sp.pitch_body = -manual.pitch * M_PI_F / 8.0f;
att_sp.yaw_body = -manual.yaw * M_PI_F;
att_sp.timestamp = hrt_absolute_time();
if (motor_test_mode) {
att_sp.roll_body = 0.0f;
att_sp.pitch_body = 0.0f;

2
apps/sensors/sensors.cpp
View File

@ -855,7 +855,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
/* Voltage in volts */
raw.battery_voltage_v = (BAT_VOL_LOWPASS_1 * (raw.battery_voltage_v + BAT_VOL_LOWPASS_2 * (buf_adc.am_data1 * _parameters.battery_voltage_scaling)));
if ((buf_adc.am_data1 * _parameters.battery_voltage_scaling) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
if ((raw.battery_voltage_v) < VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS) {
raw.battery_voltage_valid = false;
raw.battery_voltage_v = 0.f;