diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-09-19 18:14:42 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-09-19 18:14:42 +0200 |
commit | 7ac13df0810a9f31b011a069a0015f1184c93314 (patch) | |
tree | 7ff9a8c15a866b4ee3e937adaeea0b01f3d1f93e /src/modules | |
parent | a012d5be828a826918b40733130d9222a3be23b2 (diff) | |
parent | 8b992f720b79546f1c0dc9e5fdf71753221695d5 (diff) | |
download | px4-firmware-7ac13df0810a9f31b011a069a0015f1184c93314.tar.gz px4-firmware-7ac13df0810a9f31b011a069a0015f1184c93314.tar.bz2 px4-firmware-7ac13df0810a9f31b011a069a0015f1184c93314.zip |
Merge branch 'master' into rgbled_fix
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/gyro_calibration.cpp | 12 | ||||
-rw-r--r-- | src/modules/px4iofirmware/dsm.c | 21 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 2 | ||||
-rw-r--r-- | src/modules/px4iofirmware/px4io.h | 2 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 12 | ||||
-rw-r--r-- | src/modules/sensors/sensor_params.c | 6 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.c | 13 |
7 files changed, 45 insertions, 23 deletions
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 82a0349c9..369e6da62 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -163,7 +163,9 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "offset calibration done."); +#if 0 /*** --- SCALING --- ***/ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); @@ -241,9 +243,14 @@ int do_gyro_calibration(int mavlink_fd) } float gyro_scale = baseline_integral / gyro_integral; - float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); +#else + float gyro_scales[] = { 1.0f, 1.0f, 1.0f }; +#endif + + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { @@ -277,9 +284,6 @@ int do_gyro_calibration(int mavlink_fd) warn("WARNING: auto-save of params to storage failed"); } - // char buf[50]; - // sprintf(buf, "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, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); /* third beep by cal end routine */ diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 206e27db5..433976656 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,28 +243,35 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { +#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) + #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; if (dsm_fd < 0) return; -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 - // XXX implement - #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 -#else switch (cmd) { case dsm_bind_power_down: /*power down DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(0); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(0); +#endif break; case dsm_bind_power_up: /*power up DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(1); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(1); +#endif dsm_guess_format(true); break; @@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } - if (dsm_channel_shift == 11) + if (dsm_channel_shift == 11) { + /* Set the 11-bit data indicator */ *num_values |= 0x8000; + } /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. @@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * Upon receiving a full dsm frame we attempt to decode it. * * @param[out] values pointer to per channel array of decoded values - * @param[out] num_values pointer to number of raw channel values returned + * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data * @return true=decoded raw channel values updated, false=no update */ bool diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index f5fa0ba75..0e2cd1689 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -165,11 +165,13 @@ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ #define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ #define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ #define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#endif #define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ #define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 11f4d053d..66c4ca906 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -100,7 +100,9 @@ extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] #define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] #define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] +#endif #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9c95fd1c5..8cb21e54f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -145,7 +145,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#endif #ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, #else @@ -462,22 +464,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; -#ifdef POWER_RELAY1 POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); -#endif -#ifdef POWER_RELAY2 POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); -#endif -#ifdef POWER_ACC1 POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); -#endif -#ifdef POWER_ACC2 POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); -#endif break; +#endif case PX4IO_P_SETUP_VBATT_SCALE: r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 950af2eba..4d719e0e1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -164,8 +164,9 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); - +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +#endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 @@ -174,7 +175,8 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index c69de52b7..59cbcf5fb 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -48,6 +48,7 @@ #include <unistd.h> #include <systemlib/err.h> #include <errno.h> +#include <semaphore.h> #include <sys/stat.h> @@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static sem_t param_sem = { .semcount = 1 }; + /** lock the parameter store */ static void param_lock(void) { - /* XXX */ + //do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - /* XXX */ + //sem_post(¶m_sem); } /** assert that the parameter store is locked */ @@ -519,6 +522,10 @@ param_save_default(void) int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); if (fd < 0) { + /* do another attempt in case the unlink call is not synced yet */ + usleep(5000); + fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + warn("opening '%s' for writing failed", param_get_default_file()); return fd; } @@ -528,7 +535,7 @@ param_save_default(void) if (result != 0) { warn("error exporting parameters to '%s'", param_get_default_file()); - unlink(param_get_default_file()); + (void)unlink(param_get_default_file()); return result; } |