diff options
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 39 | ||||
-rw-r--r-- | apps/commander/commander.c | 76 | ||||
-rw-r--r-- | apps/commander/commander.h | 2 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 6 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 19 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 1 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 2 |
7 files changed, 89 insertions, 56 deletions
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 380ae9c15..2644a25cb 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -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 { diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4135abf60..491a658a1 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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, ¤t_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, ¤t_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, ¤t_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; diff --git a/apps/commander/commander.h b/apps/commander/commander.h index d537a0602..e9e3b24ca 100644 --- a/apps/commander/commander.h +++ b/apps/commander/commander.h @@ -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 diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index ad4976bc2..7b8a84f7e 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -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); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 0d9be58e9..1d71b4019 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -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) { diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 4f5082bed..54e25bba0 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -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; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 224474c08..c5b9e483e 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -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; |