diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-07 22:12:32 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-07 22:12:32 +0200 |
commit | b67d7fc22aaea71cda294f4f813be8f8eee4d4c3 (patch) | |
tree | c43396b5fb9b00b9f2dd9a1a079eab8055531cda /apps | |
parent | cca865eff0e4db346abb620c7c73ba54f8db4b89 (diff) | |
parent | c25cef299f7bd2e09062e38cebd94298b331e6e7 (diff) | |
download | px4-firmware-b67d7fc22aaea71cda294f4f813be8f8eee4d4c3.tar.gz px4-firmware-b67d7fc22aaea71cda294f4f813be8f8eee4d4c3.tar.bz2 px4-firmware-b67d7fc22aaea71cda294f4f813be8f8eee4d4c3.zip |
Merge branch 'master' of github.com:PX4/Firmware
Diffstat (limited to 'apps')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 54 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 44 | ||||
-rw-r--r-- | apps/commander/commander.c | 106 | ||||
-rw-r--r-- | apps/commander/commander.h | 2 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.c | 13 | ||||
-rw-r--r-- | apps/commander/state_machine_helper.h | 6 | ||||
-rw-r--r-- | apps/drivers/hmc5883/hmc5883.cpp | 6 | ||||
-rw-r--r-- | apps/drivers/mpu6000/mpu6000.cpp | 8 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 121 | ||||
-rw-r--r-- | apps/mavlink/mavlink_parameters.c | 6 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 1 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_attitude_control.c | 2 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 2 | ||||
-rw-r--r-- | apps/sensors/sensors.cpp | 83 | ||||
-rw-r--r-- | apps/systemlib/bson/tinybson.c | 2 | ||||
-rw-r--r-- | apps/systemlib/param/param.c | 4 | ||||
-rw-r--r-- | apps/uORB/objects_common.cpp | 3 | ||||
-rw-r--r-- | apps/uORB/topics/actuator_controls.h | 3 | ||||
-rw-r--r-- | apps/uORB/topics/debug_key_value.h | 75 | ||||
-rw-r--r-- | apps/uORB/topics/sensor_combined.h | 10 |
20 files changed, 391 insertions, 160 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index a865992ed..af88684bb 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -191,12 +191,26 @@ int ardrone_interface_thread_main(int argc, char *argv[]) char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n"; bool motor_test_mode = false; + int test_motor = -1; /* read commandline arguments */ for (int i = 0; i < argc && argv[i]; i++) { if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) { motor_test_mode = true; } + + if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { + if (i+1 < argc) { + int motor = atoi(argv[i+1]); + if (motor > 0 && motor < 5) { + test_motor = motor; + } else { + errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage); + } + } else { + errx(1, "missing parameter to -m 1..4\n %s", commandline_usage); + } + } } struct termios uart_config_original; @@ -205,18 +219,6 @@ int ardrone_interface_thread_main(int argc, char *argv[]) printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n"); } - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - ardrone_write = ardrone_open_uart(&uart_config_original); - - /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ - gpios = ar_multiplexing_init(); - - if (ardrone_write < 0) { - fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); - thread_running = false; - exit(ERROR); - } - /* Led animation */ int counter = 0; int led_counter = 0; @@ -237,6 +239,18 @@ int ardrone_interface_thread_main(int argc, char *argv[]) printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + ardrone_write = ardrone_open_uart(&uart_config_original); + + /* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */ + gpios = ar_multiplexing_init(); + + if (ardrone_write < 0) { + fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n"); + thread_running = false; + exit(ERROR); + } + /* initialize motors */ if (OK != ar_init_motors(ardrone_write, gpios)) { close(ardrone_write); @@ -253,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* close uarts */ close(ardrone_write); - ar_multiplexing_deinit(gpios); + //ar_multiplexing_deinit(gpios); /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ ardrone_write = ardrone_open_uart(&uart_config_original); @@ -279,7 +293,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) if (motor_test_mode) { /* set motors to idle speed */ - ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); + if (test_motor > 0 && test_motor < 5) { + int motors[4] = {0, 0, 0, 0}; + motors[test_motor - 1] = 10; + ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]); + } else { + ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10); + } + } else { /* MAIN OPERATION MODE */ @@ -289,7 +310,10 @@ int ardrone_interface_thread_main(int argc, char *argv[]) orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); - if (armed.armed) { + /* for now only spin if armed and immediately shut down + * if in failsafe + */ + if (armed.armed && !armed.failsafe) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { /* Silently lock down motor speeds to zero */ diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 380ae9c15..dd7a5655d 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" @@ -224,9 +226,7 @@ int ar_init_motors(int ardrone_uart, int gpios) for (i = 1; i < 5; ++i) { /* Initialize motors 1-4 */ errcounter += ar_select_motor(gpios, i); - - write(ardrone_uart, &(initbuf[3]), 1); - fsync(ardrone_uart); + usleep(200); /* * write 0xE0 - request status @@ -270,29 +270,34 @@ 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); + usleep(200); /* + * 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 +329,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 80d2c58f8..022003ee5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -69,6 +69,7 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/subsystem_info.h> +#include <uORB/topics/actuator_controls.h> #include <mavlink/mavlink_log.h> #include <systemlib/param/param.h> @@ -425,9 +426,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } else { /* announce and set new offset */ - char offset_output[50]; - sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); - mavlink_log_info(mavlink_fd, offset_output); + // char offset_output[50]; + // sprintf(offset_output, "[commander] mag cal: %8.4f %8.4f %8.4f", (double)mag_offset[0], (double)mag_offset[1], (double)mag_offset[2]); + // mavlink_log_info(mavlink_fd, offset_output); if (param_set(param_find("SENSOR_MAG_XOFF"), &(mag_offset[0]))) { fprintf(stderr, "[commander] Setting X mag offset failed!\n"); @@ -505,9 +506,9 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_gyro_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - char offset_output[50]; - sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - mavlink_log_info(mavlink_fd, offset_output); + // char offset_output[50]; + // sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, offset_output); close(sub_sensor_combined); } @@ -573,10 +574,10 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - char offset_output[50]; - sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], - (double)accel_offset[1], (double)accel_offset[2]); - mavlink_log_info(mavlink_fd, offset_output); + // char offset_output[50]; + // sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], + // (double)accel_offset[1], (double)accel_offset[2]); + // mavlink_log_info(mavlink_fd, offset_output); close(sub_sensor_combined); } @@ -972,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; @@ -1009,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 */ @@ -1018,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 two and a half seconds + */ + if (battery_voltage_valid && (hrt_absolute_time() - start_time > 2500000)) { + 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) { @@ -1083,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 @@ -1141,39 +1183,9 @@ 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; if (rc.chan_count > 4 && (hrt_absolute_time() - rc.timestamp) < 100000) { @@ -1238,10 +1250,18 @@ int commander_thread_main(int argc, char *argv[]) /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ current_status.rc_signal_cutting_off = true; current_status.rc_signal_lost_interval = hrt_absolute_time() - rc.timestamp; + /* if the RC signal is gone for a full second, consider it lost */ if (current_status.rc_signal_lost_interval > 1000000) current_status.rc_signal_lost = true; } + /* Check if this is the first loss or first gain*/ + if ((!prev_lost && current_status.rc_signal_lost) || + prev_lost && !current_status.rc_signal_lost) { + /* publish rc lost */ + publish_armed_status(¤t_status); + } + /* End mode switch */ /* END RC state check */ 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/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 4e2166a3a..51ed95114 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -199,10 +199,7 @@ int do_state_update(int status_pub, struct vehicle_status_s *current_status, con if (invalid_state == false || old_state != new_state) { current_status->state_machine = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + publish_armed_status(current_status); ret = OK; } if (invalid_state) { @@ -220,6 +217,14 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat printf("[commander] new state: %s\n", system_state_txt[current_status->state_machine]); } +void publish_armed_status(const struct vehicle_status_s *current_status) { + struct actuator_armed_s armed; + armed.armed = current_status->flag_system_armed; + armed.failsafe = current_status->rc_signal_lost; + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +} + /* * Private functions, update the state machine diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 4908c799a..c4d1b78a5 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -188,6 +188,12 @@ void state_machine_emergency_always_critical(int status_pub, struct vehicle_stat */ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +/** + * Publish the armed state depending on the current system state + * + * @param current_status the current system status + */ +void publish_armed_status(const struct vehicle_status_s *current_status); diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 1f05a3343..da3b83777 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -612,8 +612,8 @@ HMC5883::collect() #pragma pack(push, 1) struct { /* status register and data as read back from the device */ uint8_t x[2]; - uint8_t y[2]; uint8_t z[2]; + uint8_t y[2]; } hmc_report; #pragma pack(pop) struct { @@ -833,7 +833,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); /* set the queue depth to 10 */ @@ -863,7 +863,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("measurement: %.6f %.6f %.6f", report.x, report.y, report.z); + warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); } diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 04ba4cbbb..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); @@ -1120,5 +1122,5 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(argv[1], "info")) mpu6000::info(); - errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ef0eab130..57c459430 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -75,6 +75,7 @@ #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/manual_control_setpoint.h> +#include <uORB/topics/debug_key_value.h> #include <systemlib/param/param.h> #include "waypoints.h" @@ -166,6 +167,7 @@ static struct mavlink_subscriptions { int spa_sub; int spl_sub; int spg_sub; + int debug_key_value; bool initialized; } mavlink_subs = { .sensor_sub = 0, @@ -183,6 +185,7 @@ static struct mavlink_subscriptions { .spa_sub = 0, .spl_sub = 0, .spg_sub = 0, + .debug_key_value = 0, .initialized = false }; @@ -539,10 +542,15 @@ 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 */ if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval); + break; + case MAVLINK_MSG_ID_NAMED_VALUE_FLOAT: + if (subs->debug_key_value) orb_set_interval(subs->debug_key_value, min_interval); + break; default: /* not found */ ret = ERROR; @@ -588,6 +596,7 @@ static void *uorb_receiveloop(void *arg) struct actuator_outputs_s act_outputs; struct manual_control_setpoint_s man_control; struct actuator_controls_s actuators; + struct debug_key_value_s debug; } buf; /* --- SENSORS RAW VALUE --- */ @@ -704,11 +713,17 @@ 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++; + /* --- DEBUG VALUE OUTPUT --- */ + /* subscribe to ORB for debug value outputs */ + subs->debug_key_value = orb_subscribe(ORB_ID(debug_key_value)); + fds[fdsc_count].fd = subs->debug_key_value; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* all subscriptions initialized, return success */ subs->initialized = true; @@ -755,6 +770,38 @@ static void *uorb_receiveloop(void *arg) /* send scaled imu data (m/s^2 accelerations scaled back to milli-g) */ //mavlink_msg_scaled_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0] * 101.936799f, buf.raw.accelerometer_m_s2[1] * 101.936799f, buf.raw.accelerometer_m_s2[2] * 101.936799f, buf.raw.gyro_rad_s[0] * 1000, buf.raw.gyro_rad_s[1] * 1000, buf.raw.gyro_rad_s[2] * 1000, buf.raw.magnetometer_ga[0] * 1000, buf.raw.magnetometer_ga[1] * 1000, buf.raw.magnetometer_ga[2] * 1000); /* send scaled imu data */ + + /* mark individual fields as changed */ + uint16_t fields_updated = 0; + static unsigned accel_counter = 0; + static unsigned gyro_counter = 0; + static unsigned mag_counter = 0; + static unsigned baro_counter = 0; + + if (accel_counter != buf.raw.accelerometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 0) | (1 << 1) | (1 << 2); + accel_counter = buf.raw.accelerometer_counter; + } + + if (gyro_counter != buf.raw.gyro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 3) | (1 << 4) | (1 << 5); + gyro_counter = buf.raw.gyro_counter; + } + + if (mag_counter != buf.raw.magnetometer_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 6) | (1 << 7) | (1 << 8); + mag_counter = buf.raw.magnetometer_counter; + } + + if (baro_counter != buf.raw.baro_counter) { + /* mark first three dimensions as changed */ + fields_updated |= (1 << 9) | (1 << 11) | (1 << 12); + baro_counter = buf.raw.baro_counter; + } + mavlink_msg_highres_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_m_s2[0], buf.raw.accelerometer_m_s2[1], buf.raw.accelerometer_m_s2[2], buf.raw.gyro_rad_s[0], @@ -762,7 +809,8 @@ static void *uorb_receiveloop(void *arg) buf.raw.magnetometer_ga[0], buf.raw.magnetometer_ga[1],buf.raw.magnetometer_ga[2], buf.raw.baro_pres_mbar, 0 /* no diff pressure yet */, - buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius); + buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius, + fields_updated); /* send pressure */ //mavlink_msg_scaled_pressure_send(MAVLINK_COMM_0, buf.raw.timestamp / 1000, buf.raw.baro_pres_mbar, buf.raw.baro_alt_meter, buf.raw.baro_temp_celcius * 100); @@ -895,32 +943,15 @@ static void *uorb_receiveloop(void *arg) /* Only send in HIL mode */ if (mavlink_hil_enabled) { - /* hacked HIL implementation in order for the APM Planner to work - * (correct cmd: mavlink_msg_hil_controls_send()) - */ - - mavlink_msg_rc_channels_scaled_send(chan, - hrt_absolute_time(), - 0, // port 0 - buf.att_sp.roll_body, - buf.att_sp.pitch_body, - buf.att_sp.thrust, - buf.att_sp.yaw_body, - 0, - 0, - 0, - 0, - 1 /*rssi=1*/); - /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); - /* correct HIL message as per MAVLink spec */ + /* HIL message as per MAVLink spec */ mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), - buf.att_sp.roll_body, + buf.att_sp.roll_body, /* this may be replaced by ctrl0 later */ buf.att_sp.pitch_body, buf.att_sp.yaw_body, buf.att_sp.thrust, @@ -1060,10 +1091,17 @@ static void *uorb_receiveloop(void *arg) /* --- ACTUATOR CONTROL --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, subs->actuators_sub, &buf.actuators); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0", buf.actuators.control[0]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1", buf.actuators.control[1]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2", buf.actuators.control[2]); - mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3", buf.actuators.control[3]); + /* send, add spaces so that string buffer is at least 10 chars long */ + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl0 ", buf.actuators.control[0]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl1 ", buf.actuators.control[1]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl2 ", buf.actuators.control[2]); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, "ctrl3 ", buf.actuators.control[3]); + } + + /* --- DEBUG KEY/VALUE --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(debug_key_value), subs->debug_key_value, &buf.debug); + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, hrt_absolute_time() / 1000, buf.debug.key, buf.debug.value); } } } @@ -1495,28 +1533,33 @@ 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_HIGHRES_IMU, 5); /* 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); - /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20); + /* 50 Hz / 10 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20); + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + 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) { /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 20); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); + /* 20 Hz / 50 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); /* 10 Hz / 100 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100); /* 1 Hz */ @@ -1527,6 +1570,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 100); /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200); + /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 0.2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000); @@ -1535,6 +1580,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000); + /* 1 Hz / 1000 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000); /* 0.5 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); /* 0.1 Hz */ diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c index 8278837e8..2e15174bb 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -189,7 +189,7 @@ static int mavlink_pm_save_eeprom() int result = param_export(fd, false); close(fd); - if (result < 0) { + if (result != 0) { unlink(mavlink_parameter_file); warn("error exporting parameters to '%s'", mavlink_parameter_file); return -2; @@ -218,7 +218,7 @@ mavlink_pm_load_eeprom() int result = param_load(fd); close(fd); - if (result < 0) { + if (result != 0) { warn("error reading parameters from '%s'", mavlink_parameter_file); return -2; } @@ -346,7 +346,7 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess } /* send back command result */ - // mavlink_message_t tx; + //mavlink_msg_command_ack_send(chan, cmd.command, result); } break; } } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 23373b998..350308fa3 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/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index c6547b199..312942acb 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -231,7 +231,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&yaw_pos_controller, p.yaw_p, p.yaw_i, p.yaw_d, p.yaw_awu); pid_set_parameters(&yaw_speed_controller, p.yawrate_p, p.yawrate_d, p.yawrate_i, p.yawrate_awu); pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu); - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, p.att_awu); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, p.att_awu); } /* calculate current control outputs */ diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 0f6b6044f..45267f315 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -226,7 +226,7 @@ int attitude_estimator_bm_main(int argc, char *argv[]) att.timestamp = sensor_combined_s_local.timestamp; att.roll = euler.x; att.pitch = euler.y; - att.yaw = euler.z - M_PI_F; + att.yaw = euler.z; if (att.yaw > M_PI_F) { att.yaw -= 2.0f * M_PI_F; diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 79f9c19e7..963c54b8e 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -653,9 +653,16 @@ Sensors::accel_poll(struct sensor_combined_s &raw) accel_report.x = (((accel_report.x_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f; accel_report.y = (((accel_report.y_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f; accel_report.z = (((accel_report.z_raw - _parameters.acc_offset[0]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_counter++; } else { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + bool accel_updated; + orb_check(_accel_sub, &accel_updated); + + if (accel_updated) { + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); + raw.accelerometer_counter++; + } } raw.accelerometer_m_s2[0] = accel_report.x; @@ -665,8 +672,6 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; - - raw.accelerometer_raw_counter++; } void @@ -691,9 +696,6 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) gyro_report.y = (gyro_report.y_raw - _parameters.gyro_offset[1]) * 0.000266316109f; gyro_report.z = (gyro_report.z_raw - _parameters.gyro_offset[2]) * 0.000266316109f; - } else { - orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); - raw.gyro_rad_s[0] = gyro_report.x; raw.gyro_rad_s[1] = gyro_report.y; raw.gyro_rad_s[2] = gyro_report.z; @@ -702,40 +704,69 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; - raw.gyro_raw_counter++; + raw.gyro_counter++; + + } else { + + bool gyro_updated; + orb_check(_gyro_sub, &gyro_updated); + + if (gyro_updated) { + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); + + raw.gyro_rad_s[0] = gyro_report.x; + raw.gyro_rad_s[1] = gyro_report.y; + raw.gyro_rad_s[2] = gyro_report.z; + + raw.gyro_raw[0] = gyro_report.x_raw; + raw.gyro_raw[1] = gyro_report.y_raw; + raw.gyro_raw[2] = gyro_report.z_raw; + + raw.gyro_counter++; + } } } void Sensors::mag_poll(struct sensor_combined_s &raw) { - struct mag_report mag_report; + bool mag_updated; + orb_check(_mag_sub, &mag_updated); - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); + if (mag_updated) { + struct mag_report mag_report; - raw.magnetometer_ga[0] = mag_report.x; - raw.magnetometer_ga[1] = mag_report.y; - raw.magnetometer_ga[2] = mag_report.z; + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - raw.magnetometer_raw[0] = mag_report.x_raw; - raw.magnetometer_raw[1] = mag_report.y_raw; - raw.magnetometer_raw[2] = mag_report.z_raw; - - raw.magnetometer_raw_counter++; + raw.magnetometer_ga[0] = mag_report.x; + raw.magnetometer_ga[1] = mag_report.y; + raw.magnetometer_ga[2] = mag_report.z; + + raw.magnetometer_raw[0] = mag_report.x_raw; + raw.magnetometer_raw[1] = mag_report.y_raw; + raw.magnetometer_raw[2] = mag_report.z_raw; + + raw.magnetometer_counter++; + } } void Sensors::baro_poll(struct sensor_combined_s &raw) { - struct baro_report baro_report; + bool baro_updated; + orb_check(_baro_sub, &baro_updated); + + if (baro_updated) { + struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &baro_report); - raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar - raw.baro_alt_meter = baro_report.altitude; // Altitude in meters - raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + raw.baro_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro_temp_celcius = baro_report.temperature; // Temperature in degrees celcius - raw.baro_raw_counter++; + raw.baro_counter++; + } } void @@ -779,8 +810,6 @@ Sensors::parameter_update_poll(bool forced) struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), _params_sub, &update); - printf("PARAMS UPDATED\n"); - /* update parameters */ parameters_update(); @@ -824,7 +853,7 @@ Sensors::parameter_update_poll(bool forced) warn("WARNING: failed to set scale / offsets for mag"); close(fd); -#if 1 +#if 0 printf("CH0: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[0], (int)_parameters.min[0], (int)(_rc.chan[0].scaling_factor*10000), (int)(_rc.chan[0].mid), (int)_rc.function[0]); printf("CH1: RAW MAX: %d MIN %d S: %d MID: %d FUNC: %d\n", (int)_parameters.max[1], (int)_parameters.min[1], (int)(_rc.chan[1].scaling_factor*10000), (int)(_rc.chan[1].mid), (int)_rc.function[1]); printf("MAN: %d %d\n", (int)(_rc.chan[0].scaled*100), (int)(_rc.chan[1].scaled*100)); @@ -857,7 +886,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; diff --git a/apps/systemlib/bson/tinybson.c b/apps/systemlib/bson/tinybson.c index 27878401e..46ced013d 100644 --- a/apps/systemlib/bson/tinybson.c +++ b/apps/systemlib/bson/tinybson.c @@ -44,7 +44,7 @@ #include "tinybson.h" -#if 1 +#if 0 # define debug(fmt, args...) do { warnx("BSON: " fmt, ##args); } while(0) #else # define debug(fmt, args...) do { } while(0) diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 3db55f967..a01c170a6 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -479,7 +479,7 @@ param_export(int fd, bool only_unsaved) * If we are only saving values changed since last save, and this * one hasn't, then skip it */ - if (only_unsaved & !s->unsaved) + if (only_unsaved && !s->unsaved) continue; s->unsaved = false; @@ -519,7 +519,7 @@ param_export(int fd, bool only_unsaved) break; default: - debug("unrecognised parameter type"); + debug("unrecognized parameter type"); goto out; } } diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index de26dcf01..e40199e97 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -123,3 +123,6 @@ ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); + +#include "topics/debug_key_value.h" +ORB_DEFINE(debug_key_value, struct debug_key_value_s); diff --git a/apps/uORB/topics/actuator_controls.h b/apps/uORB/topics/actuator_controls.h index 2b7d7de5d..e6d7c8670 100644 --- a/apps/uORB/topics/actuator_controls.h +++ b/apps/uORB/topics/actuator_controls.h @@ -67,7 +67,8 @@ ORB_DECLARE(actuator_controls_3); /** global 'actuator output is live' control. */ struct actuator_armed_s { - bool armed; + bool armed; /**< Set to true if system is armed */ + bool failsafe; /**< Set to true if no valid control input is available */ }; ORB_DECLARE(actuator_armed); diff --git a/apps/uORB/topics/debug_key_value.h b/apps/uORB/topics/debug_key_value.h new file mode 100644 index 000000000..a9d1b83fd --- /dev/null +++ b/apps/uORB/topics/debug_key_value.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier <lm@inf.ethz.ch> + * + * 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 debug_key_value.h + * Definition of the debug named value uORB topic. Allows to send a 10-char key + * with a float as value. + */ + +#ifndef TOPIC_DEBUG_KEY_VALUE_H_ +#define TOPIC_DEBUG_KEY_VALUE_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + */ + +/** + * Key/value pair for debugging + */ +struct debug_key_value_s { + + /* + * Actual data, this is specific to the type of data which is stored in this struct + * A line containing L0GME will be added by the Python logging code generator to the + * logged dataset. + */ + uint32_t timestamp_ms; /**< in milliseconds since system start */ + char key[10]; /**< max. 10 characters as key / name */ + float value; /**< the value to send as debug output */ + +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(debug_key_value); + +#endif diff --git a/apps/uORB/topics/sensor_combined.h b/apps/uORB/topics/sensor_combined.h index 8fae78858..8c1847753 100644 --- a/apps/uORB/topics/sensor_combined.h +++ b/apps/uORB/topics/sensor_combined.h @@ -80,11 +80,11 @@ struct sensor_combined_s { uint64_t timestamp; /**< Timestamp in microseconds since boot LOGME */ int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity LOGME */ - uint16_t gyro_raw_counter; /**< Number of raw measurments taken LOGME */ + uint16_t gyro_counter; /**< Number of raw measurments taken LOGME */ float gyro_rad_s[3]; /**< Angular velocity in radian per seconds LOGME */ int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame LOGME */ - uint16_t accelerometer_raw_counter; /**< Number of raw acc measurements taken LOGME */ + uint32_t accelerometer_counter; /**< Number of raw acc measurements taken LOGME */ float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 LOGME */ int accelerometer_mode; /**< Accelerometer measurement mode */ float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ @@ -94,15 +94,15 @@ struct sensor_combined_s { int magnetometer_mode; /**< Magnetometer measurement mode */ float magnetometer_range_ga; /**< ± measurement range in Gauss */ float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint16_t magnetometer_raw_counter; /**< Number of raw mag measurements taken LOGME */ + uint32_t magnetometer_counter; /**< Number of raw mag measurements taken LOGME */ float baro_pres_mbar; /**< Barometric pressure, already temp. comp. LOGME */ float baro_alt_meter; /**< Altitude, already temp. comp. LOGME */ float baro_temp_celcius; /**< Temperature in degrees celsius LOGME */ float battery_voltage_v; /**< Battery voltage in volts, filtered LOGME */ float adc_voltage_v[3]; /**< ADC voltages of ADC Chan 11/12/13 or -1 LOGME */ - uint16_t baro_raw_counter; /**< Number of raw baro measurements taken LOGME */ - uint16_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ + uint32_t baro_counter; /**< Number of raw baro measurements taken LOGME */ + uint32_t battery_voltage_counter; /**< Number of voltage measurements taken LOGME */ bool battery_voltage_valid; /**< True if battery voltage can be measured LOGME */ }; |