diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-17 09:50:22 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-01-17 09:50:22 +0100 |
commit | 73546b6645c5715ba0cecf00899632bf861321c9 (patch) | |
tree | d0eb82de70d6cc02ef0e6a3ccfd89525478ab8e5 /src/modules/systemlib | |
parent | 14c0fae175452da6e28ffc20b265de621a2430ba (diff) | |
parent | e691bab71a69216273fdc253695ef849671af9a7 (diff) | |
download | px4-firmware-73546b6645c5715ba0cecf00899632bf861321c9.tar.gz px4-firmware-73546b6645c5715ba0cecf00899632bf861321c9.tar.bz2 px4-firmware-73546b6645c5715ba0cecf00899632bf861321c9.zip |
Merge remote-tracking branch 'upstream/master' into navigator_new
Conflicts:
makefiles/config_px4fmu-v1_backside.mk
src/modules/commander/commander.cpp
src/modules/sdlog2/sdlog2.c
Diffstat (limited to 'src/modules/systemlib')
-rw-r--r-- | src/modules/systemlib/bson/tinybson.c | 3 | ||||
-rw-r--r-- | src/modules/systemlib/module.mk | 3 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.c | 35 | ||||
-rw-r--r-- | src/modules/systemlib/pwm_limit/pwm_limit.c | 114 | ||||
-rw-r--r-- | src/modules/systemlib/pwm_limit/pwm_limit.h | 20 |
5 files changed, 111 insertions, 64 deletions
diff --git a/src/modules/systemlib/bson/tinybson.c b/src/modules/systemlib/bson/tinybson.c index 8aca6a25d..49403c98b 100644 --- a/src/modules/systemlib/bson/tinybson.c +++ b/src/modules/systemlib/bson/tinybson.c @@ -407,6 +407,9 @@ bson_encoder_fini(bson_encoder_t encoder) memcpy(encoder->buf, &len, sizeof(len)); } + /* sync file */ + fsync(encoder->fd); + return 0; } diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 8c6c300d6..3953b757d 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -51,5 +51,6 @@ SRCS = err.c \ mavlink_log.c \ rc_check.c \ otp.c \ - board_serial.c + board_serial.c \ + pwm_limit/pwm_limit.c diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 398657dd7..2d773fd25 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -61,7 +61,7 @@ #include "uORB/uORB.h" #include "uORB/topics/parameter_update.h" -#if 1 +#if 0 # define debug(fmt, args...) do { warnx(fmt, ##args); } while(0) #else # define debug(fmt, args...) do { } while(0) @@ -512,6 +512,28 @@ param_save_default(void) int fd; const char *filename = param_get_default_file(); + + /* write parameters to temp file */ + fd = open(filename, O_WRONLY | O_CREAT); + + if (fd < 0) { + warn("failed to open param file: %s", filename); + return ERROR; + } + + if (res == OK) { + res = param_export(fd, false); + + if (res != OK) { + warnx("failed to write parameters to file: %s", filename); + } + } + + close(fd); + + return res; + +#if 0 const char *filename_tmp = malloc(strlen(filename) + 5); sprintf(filename_tmp, "%s.tmp", filename); @@ -565,6 +587,7 @@ param_save_default(void) free(filename_tmp); return res; +#endif } /** @@ -573,9 +596,9 @@ param_save_default(void) int param_load_default(void) { - int fd = open(param_get_default_file(), O_RDONLY); + int fd_load = open(param_get_default_file(), O_RDONLY); - if (fd < 0) { + if (fd_load < 0) { /* no parameter file is OK, otherwise this is an error */ if (errno != ENOENT) { warn("open '%s' for reading failed", param_get_default_file()); @@ -584,8 +607,8 @@ param_load_default(void) return 1; } - int result = param_load(fd); - close(fd); + int result = param_load(fd_load); + close(fd_load); if (result != 0) { warn("error reading parameters from '%s'", param_get_default_file()); diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c index cac3dc82a..190b315f1 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.c +++ b/src/modules/systemlib/pwm_limit/pwm_limit.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. * Author: Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -44,38 +44,53 @@ #include <math.h> #include <stdbool.h> #include <drivers/drv_hrt.h> +#include <stdio.h> void pwm_limit_init(pwm_limit_t *limit) { - limit->state = LIMIT_STATE_OFF; + limit->state = PWM_LIMIT_STATE_INIT; limit->time_armed = 0; return; } -void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit) +void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit) { + /* first evaluate state changes */ switch (limit->state) { - case LIMIT_STATE_OFF: - if (armed) - limit->state = LIMIT_STATE_RAMP; - limit->time_armed = hrt_absolute_time(); + case PWM_LIMIT_STATE_INIT: + + if (armed) { + + /* set arming time for the first call */ + if (limit->time_armed == 0) { + limit->time_armed = hrt_absolute_time(); + } + + if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { + limit->state = PWM_LIMIT_STATE_OFF; + } + } break; - case LIMIT_STATE_INIT: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US) - limit->state = LIMIT_STATE_RAMP; + case PWM_LIMIT_STATE_OFF: + if (armed) { + limit->state = PWM_LIMIT_STATE_RAMP; + + /* reset arming time, used for ramp timing */ + limit->time_armed = hrt_absolute_time(); + } break; - case LIMIT_STATE_RAMP: - if (!armed) - limit->state = LIMIT_STATE_OFF; - else if (hrt_absolute_time() - limit->time_armed >= INIT_TIME_US + RAMP_TIME_US) - limit->state = LIMIT_STATE_ON; + case PWM_LIMIT_STATE_RAMP: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) { + limit->state = PWM_LIMIT_STATE_ON; + } break; - case LIMIT_STATE_ON: - if (!armed) - limit->state = LIMIT_STATE_OFF; + case PWM_LIMIT_STATE_ON: + if (!armed) { + limit->state = PWM_LIMIT_STATE_OFF; + } break; default: break; @@ -86,44 +101,47 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_ /* then set effective_pwm based on state */ switch (limit->state) { - case LIMIT_STATE_OFF: - case LIMIT_STATE_INIT: + case PWM_LIMIT_STATE_OFF: + case PWM_LIMIT_STATE_INIT: for (unsigned i=0; i<num_channels; i++) { effective_pwm[i] = disarmed_pwm[i]; - output[i] = 0.0f; } break; - case LIMIT_STATE_RAMP: + case PWM_LIMIT_STATE_RAMP: + { + hrt_abstime diff = hrt_elapsed_time(&limit->time_armed); - progress = (hrt_absolute_time() - INIT_TIME_US - limit->time_armed)*10000 / RAMP_TIME_US; - for (unsigned i=0; i<num_channels; i++) { - - uint16_t ramp_min_pwm; - - /* if a disarmed pwm value was set, blend between disarmed and min */ - if (disarmed_pwm[i] > 0) { - - /* safeguard against overflows */ - uint16_t disarmed = disarmed_pwm[i]; - if (disarmed > min_pwm[i]) - disarmed = min_pwm[i]; - - uint16_t disarmed_min_diff = min_pwm[i] - disarmed; - ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; - } else { - - /* no disarmed pwm value set, choose min pwm */ - ramp_min_pwm = min_pwm[i]; - } + progress = diff * 10000 / RAMP_TIME_US; + + for (unsigned i=0; i<num_channels; i++) { + + uint16_t ramp_min_pwm; + + /* if a disarmed pwm value was set, blend between disarmed and min */ + if (disarmed_pwm[i] > 0) { - effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; - output[i] = (float)progress/10000.0f * output[i]; + /* safeguard against overflows */ + unsigned disarmed = disarmed_pwm[i]; + if (disarmed > min_pwm[i]) { + disarmed = min_pwm[i]; + } + + unsigned disarmed_min_diff = min_pwm[i] - disarmed; + ramp_min_pwm = disarmed + (disarmed_min_diff * progress) / 10000; + + } else { + + /* no disarmed pwm value set, choose min pwm */ + ramp_min_pwm = min_pwm[i]; + } + + effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2; + } } break; - case LIMIT_STATE_ON: + case PWM_LIMIT_STATE_ON: for (unsigned i=0; i<num_channels; i++) { effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2; - /* effective_output stays the same */ } break; default: diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.h b/src/modules/systemlib/pwm_limit/pwm_limit.h index 9974770be..6a667ac6f 100644 --- a/src/modules/systemlib/pwm_limit/pwm_limit.h +++ b/src/modules/systemlib/pwm_limit/pwm_limit.h @@ -46,6 +46,8 @@ #include <stdint.h> #include <stdbool.h> +__BEGIN_DECLS + /* * time for the ESCs to initialize * (this is not actually needed if PWM is sent right after boot) @@ -56,21 +58,21 @@ */ #define RAMP_TIME_US 2500000 +enum pwm_limit_state { + PWM_LIMIT_STATE_OFF = 0, + PWM_LIMIT_STATE_INIT, + PWM_LIMIT_STATE_RAMP, + PWM_LIMIT_STATE_ON +}; + typedef struct { - enum { - LIMIT_STATE_OFF = 0, - LIMIT_STATE_INIT, - LIMIT_STATE_RAMP, - LIMIT_STATE_ON - } state; + enum pwm_limit_state state; uint64_t time_armed; } pwm_limit_t; -__BEGIN_DECLS - __EXPORT void pwm_limit_init(pwm_limit_t *limit); -__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, float *output, uint16_t *effective_pwm, pwm_limit_t *limit); +__EXPORT void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_t *disarmed_pwm, const uint16_t *min_pwm, const uint16_t *max_pwm, const float *output, uint16_t *effective_pwm, pwm_limit_t *limit); __END_DECLS |