diff options
Diffstat (limited to 'src/modules/systemlib')
-rw-r--r-- | src/modules/systemlib/board_serial.c | 60 | ||||
-rw-r--r-- | src/modules/systemlib/board_serial.h | 49 | ||||
-rw-r--r-- | src/modules/systemlib/bson/tinybson.c | 3 | ||||
-rw-r--r-- | src/modules/systemlib/module.mk | 6 | ||||
-rw-r--r-- | src/modules/systemlib/otp.c | 224 | ||||
-rw-r--r-- | src/modules/systemlib/otp.h | 151 | ||||
-rw-r--r-- | src/modules/systemlib/param/param.c | 35 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.c | 106 | ||||
-rw-r--r-- | src/modules/systemlib/pid/pid.h | 41 | ||||
-rw-r--r-- | src/modules/systemlib/pwm_limit/pwm_limit.c | 114 | ||||
-rw-r--r-- | src/modules/systemlib/pwm_limit/pwm_limit.h | 20 |
11 files changed, 654 insertions, 155 deletions
diff --git a/src/modules/systemlib/board_serial.c b/src/modules/systemlib/board_serial.c new file mode 100644 index 000000000..ad8c2bf83 --- /dev/null +++ b/src/modules/systemlib/board_serial.c @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 + * 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 board_serial.h + * Read off the board serial + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> + * + */ + +#include "otp.h" +#include "board_config.h" +#include "board_serial.h" + +int get_board_serial(char *serialid) +{ + const volatile unsigned *udid_ptr = (const unsigned *)UDID_START; + union udid id; + val_read((unsigned *)&id, udid_ptr, sizeof(id)); + + + /* Copy the serial from the chips non-write memory and swap endianess */ + serialid[0] = id.data[3]; serialid[1] = id.data[2]; serialid[2] = id.data[1]; serialid[3] = id.data[0]; + serialid[4] = id.data[7]; serialid[5] = id.data[6]; serialid[6] = id.data[5]; serialid[7] = id.data[4]; + serialid[8] = id.data[11]; serialid[9] = id.data[10]; serialid[10] = id.data[9]; serialid[11] = id.data[8]; + + return 0; +}
\ No newline at end of file diff --git a/src/modules/systemlib/board_serial.h b/src/modules/systemlib/board_serial.h new file mode 100644 index 000000000..b14bb4376 --- /dev/null +++ b/src/modules/systemlib/board_serial.h @@ -0,0 +1,49 @@ +/**************************************************************************** + * + * 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 + * 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 board_serial.h + * Read off the board serial + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> + * + */ + +#pragma once + +__BEGIN_DECLS + +__EXPORT int get_board_serial(char *serialid); + +__END_DECLS 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 843cda722..3953b757d 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -49,4 +49,8 @@ SRCS = err.c \ airspeed.c \ system_params.c \ mavlink_log.c \ - rc_check.c + rc_check.c \ + otp.c \ + board_serial.c \ + pwm_limit/pwm_limit.c + diff --git a/src/modules/systemlib/otp.c b/src/modules/systemlib/otp.c new file mode 100644 index 000000000..695574fdc --- /dev/null +++ b/src/modules/systemlib/otp.c @@ -0,0 +1,224 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. + * Authors: + * Lorenz Meier <lm@inf.ethz.ch> + * David "Buzz" Bussenschutt <davidbuzz@gmail.com> + * + * 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 otp.c + * otp estimation + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> + * + */ + +#include <nuttx/config.h> +#include <board_config.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> +#include <string.h> // memset +#include "conversions.h" +#include "otp.h" +#include "err.h" // warnx +#include <assert.h> + + +int val_read(void *dest, volatile const void *src, int bytes) +{ + + int i; + + for (i = 0; i < bytes / 4; i++) { + *(((volatile unsigned *)dest) + i) = *(((volatile unsigned *)src) + i); + } + + return i * 4; +} + + +int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature) +{ + + warnx("write_otp: PX4 / %02X / %02X / %02X / ... etc \n", id_type, vid, pid); + + int errors = 0; + + // descriptor + if (F_write_byte(ADDR_OTP_START, 'P')) + errors++; + // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 1, 'X')) + errors++; // write the 'P' from PX4. to first byte in OTP + if (F_write_byte(ADDR_OTP_START + 2, '4')) + errors++; + if (F_write_byte(ADDR_OTP_START + 3, '\0')) + errors++; + //id_type + if (F_write_byte(ADDR_OTP_START + 4, id_type)) + errors++; + // vid and pid are 4 bytes each + if (F_write_word(ADDR_OTP_START + 5, vid)) + errors++; + if (F_write_word(ADDR_OTP_START + 9, pid)) + errors++; + + // leave some 19 bytes of space, and go to the next block... + // then the auth sig starts + for (int i = 0 ; i < 128 ; i++) { + if (F_write_byte(ADDR_OTP_START + 32 + i, signature[i])) + errors++; + } + + return errors; +} + +int lock_otp(void) +{ + //determine the required locking size - can only write full lock bytes */ +// int size = sizeof(struct otp) / 32; +// +// struct otp_lock otp_lock_mem; +// +// memset(&otp_lock_mem, OTP_LOCK_UNLOCKED, sizeof(otp_lock_mem)); +// for (int i = 0; i < sizeof(otp_lock_mem) / sizeof(otp_lock_mem.lock_bytes[0]); i++) +// otp_lock_mem.lock_bytes[i] = OTP_LOCK_LOCKED; + //XXX add the actual call here to write the OTP_LOCK bytes only at final stage + // val_copy(lock_ptr, &otp_lock_mem, sizeof(otp_lock_mem)); + + int locksize = 5; + + int errors = 0; + + // or just realise it's exctly 5x 32byte blocks we need to lock. 1 block for ID,type,vid,pid, and 4 blocks for certificate, which is 128 bytes. + for (int i = 0 ; i < locksize ; i++) { + if (F_write_byte(ADDR_OTP_LOCK_START + i, OTP_LOCK_LOCKED)) + errors++; + } + + return errors; +} + + + +// COMPLETE, BUSY, or other flash error? +int F_GetStatus(void) +{ + int fs = F_COMPLETE; + + if ((FLASH->status & F_BSY) == F_BSY) { fs = F_BUSY; } else { + + if ((FLASH->status & F_WRPERR) != (uint32_t)0x00) { fs = F_ERROR_WRP; } else { + + if ((FLASH->status & (uint32_t)0xEF) != (uint32_t)0x00) { fs = F_ERROR_PROGRAM; } else { + + if ((FLASH->status & F_OPERR) != (uint32_t)0x00) { fs = F_ERROR_OPERATION; } else { + fs = F_COMPLETE; + } + } + } + } + + return fs; +} + + +// enable FLASH Registers +void F_unlock(void) +{ + if ((FLASH->control & F_CR_LOCK) != 0) { + FLASH->key = F_KEY1; + FLASH->key = F_KEY2; + } +} + +// lock the FLASH Registers +void F_lock(void) +{ + FLASH->control |= F_CR_LOCK; +} + +// flash write word. +int F_write_word(uint32_t Address, uint32_t Data) +{ + unsigned char octet[4] = {0, 0, 0, 0}; + + int ret = 0; + + for (int i = 0; i < 4; i++) { + octet[i] = (Data >> (i * 8)) & 0xFF; + ret = F_write_byte(Address + i, octet[i]); + } + + return ret; +} + +// flash write byte +int F_write_byte(uint32_t Address, uint8_t Data) +{ + volatile int status = F_COMPLETE; + + //warnx("F_write_byte: %08X %02d", Address , Data ) ; + + //Check the parameters + assert(IS_F_ADDRESS(Address)); + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + if (status == F_COMPLETE) { + //if the previous operation is completed, proceed to program the new data + FLASH->control &= CR_PSIZE_MASK; + FLASH->control |= F_PSIZE_BYTE; + FLASH->control |= F_CR_PG; + + *(volatile uint8_t *)Address = Data; + + //Wait for FLASH operation to complete by polling on BUSY flag. + status = F_GetStatus(); + + while (status == F_BUSY) { status = F_GetStatus();} + + //if the program operation is completed, disable the PG Bit + FLASH->control &= (~F_CR_PG); + } + + //Return the Program Status + return !(status == F_COMPLETE); +} + + + diff --git a/src/modules/systemlib/otp.h b/src/modules/systemlib/otp.h new file mode 100644 index 000000000..f10e129d8 --- /dev/null +++ b/src/modules/systemlib/otp.h @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 + * 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 otp.h + * One TIme Programmable ( OTP ) Flash routine/s. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author David "Buzz" Bussenschutt <davidbuzz@gmail.com> + * + */ + +#ifndef OTP_H_ +#define OTP_H_ + +__BEGIN_DECLS + +#define ADDR_OTP_START 0x1FFF7800 +#define ADDR_OTP_LOCK_START 0x1FFF7A00 + +#define OTP_LOCK_LOCKED 0x00 +#define OTP_LOCK_UNLOCKED 0xFF + + + +#include <unistd.h> +#include <stdio.h> + +// possible flash statuses +#define F_BUSY 1 +#define F_ERROR_WRP 2 +#define F_ERROR_PROGRAM 3 +#define F_ERROR_OPERATION 4 +#define F_COMPLETE 5 + +typedef struct { + volatile uint32_t accesscontrol; // 0x00 + volatile uint32_t key; // 0x04 + volatile uint32_t optionkey; // 0x08 + volatile uint32_t status; // 0x0C + volatile uint32_t control; // 0x10 + volatile uint32_t optioncontrol; //0x14 +} flash_registers; + +#define PERIPH_BASE ((uint32_t)0x40000000) //Peripheral base address +#define AHB1PERIPH_BASE (PERIPH_BASE + 0x00020000) +#define F_R_BASE (AHB1PERIPH_BASE + 0x3C00) +#define FLASH ((flash_registers *) F_R_BASE) + +#define F_BSY ((uint32_t)0x00010000) //FLASH Busy flag bit +#define F_OPERR ((uint32_t)0x00000002) //FLASH operation Error flag bit +#define F_WRPERR ((uint32_t)0x00000010) //FLASH Write protected error flag bit +#define CR_PSIZE_MASK ((uint32_t)0xFFFFFCFF) +#define F_PSIZE_WORD ((uint32_t)0x00000200) +#define F_PSIZE_BYTE ((uint32_t)0x00000000) +#define F_CR_PG ((uint32_t)0x00000001) // a bit in the F_CR register +#define F_CR_LOCK ((uint32_t)0x80000000) // also another bit. + +#define F_KEY1 ((uint32_t)0x45670123) +#define F_KEY2 ((uint32_t)0xCDEF89AB) +#define IS_F_ADDRESS(ADDRESS) ((((ADDRESS) >= 0x08000000) && ((ADDRESS) < 0x080FFFFF)) || (((ADDRESS) >= 0x1FFF7800) && ((ADDRESS) < 0x1FFF7A0F))) + + + +#pragma pack(push, 1) + +/* + * The OTP area is divided into 16 OTP data blocks of 32 bytes and one lock OTP block of 16 bytes. + * The OTP data and lock blocks cannot be erased. The lock block contains 16 bytes LOCKBi (0 ≤ i ≤ 15) + * to lock the corresponding OTP data block (blocks 0 to 15). Each OTP data block can be programmed + * until the value 0x00 is programmed in the corresponding OTP lock byte. The lock bytes must only + * contain 0x00 and 0xFF values, otherwise the OTP bytes might not be taken into account correctly. + */ + +struct otp { + // first 32 bytes = the '0' Block + char id[4]; ///4 bytes < 'P' 'X' '4' '\n' + uint8_t id_type; ///1 byte < 0 for USB VID, 1 for generic VID + uint32_t vid; ///4 bytes + uint32_t pid; ///4 bytes + char unused[19]; ///19 bytes + // Cert-of-Auth is next 4 blocks ie 1-4 ( where zero is first block ) + char signature[128]; + // insert extras here + uint32_t lock_bytes[4]; +}; + +struct otp_lock { + uint8_t lock_bytes[16]; +}; +#pragma pack(pop) + +#define ADDR_F_SIZE 0x1FFF7A22 + +#pragma pack(push, 1) +union udid { + uint32_t serial[3]; + char data[12]; +}; +#pragma pack(pop) + + +/** + * s + */ +//__EXPORT float calc_indicated_airspeed(float differential_pressure); + +__EXPORT void F_unlock(void); +__EXPORT void F_lock(void); +__EXPORT int val_read(void *dest, volatile const void *src, int bytes); +__EXPORT int val_write(volatile void *dest, const void *src, int bytes); +__EXPORT int write_otp(uint8_t id_type, uint32_t vid, uint32_t pid, char *signature); +__EXPORT int lock_otp(void); + + +__EXPORT int F_write_byte(uint32_t Address, uint8_t Data); +__EXPORT int F_write_word(uint32_t Address, uint32_t Data); + +__END_DECLS + +#endif 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/pid/pid.c b/src/modules/systemlib/pid/pid.c index 77c952f52..6a4e9392a 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -39,7 +39,7 @@ /** * @file pid.c * - * Implementation of generic PID control interface. + * Implementation of generic PID controller. * * @author Laurens Mackay <mackayl@student.ethz.ch> * @author Tobias Naegeli <naegelit@student.ethz.ch> @@ -53,24 +53,21 @@ #define SIGMA 0.000001f -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode, float dt_min) +__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min) { - pid->kp = kp; - pid->ki = ki; - pid->kd = kd; - pid->intmax = intmax; - pid->limit = limit; pid->mode = mode; pid->dt_min = dt_min; - pid->count = 0.0f; - pid->saturated = 0.0f; - pid->last_output = 0.0f; - pid->sp = 0.0f; - pid->error_previous = 0.0f; + pid->kp = 0.0f; + pid->ki = 0.0f; + pid->kd = 0.0f; pid->integral = 0.0f; + pid->integral_limit = 0.0f; + pid->output_limit = 0.0f; + pid->error_previous = 0.0f; + pid->last_output = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) + +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit) { int ret = 0; @@ -95,15 +92,15 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } - if (isfinite(intmax)) { - pid->intmax = intmax; + if (isfinite(integral_limit)) { + pid->integral_limit = integral_limit; } else { ret = 1; } - if (isfinite(limit)) { - pid->limit = limit; + if (isfinite(output_limit)) { + pid->output_limit = output_limit; } else { ret = 1; @@ -112,42 +109,18 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float return ret; } -//void pid_set(PID_t *pid, float sp) -//{ -// pid->sp = sp; -// pid->error_previous = 0; -// pid->integral = 0; -//} - -/** - * - * @param pid - * @param val - * @param dt - * @return - */ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) { - /* error = setpoint - actual_position - integral = integral + (error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + (Ki*integral) + (Kd*derivative) - previous_error = error - wait(dt) - goto start - */ - if (!isfinite(sp) || !isfinite(val) || !isfinite(val_dot) || !isfinite(dt)) { return pid->last_output; } float i, d; - pid->sp = sp; - // Calculated current error value - float error = pid->sp - val; + /* current error value */ + float error = sp - val; - // Calculate or measured current error derivative + /* current error derivative */ if (pid->mode == PID_MODE_DERIVATIV_CALC) { d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); pid->error_previous = error; @@ -167,39 +140,34 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo d = 0.0f; } - if (pid->ki > 0.0f) { + /* calculate PD output */ + float output = (error * pid->kp) + (d * pid->kd); + + if (pid->ki > SIGMA) { // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || - fabsf(i) > pid->intmax) { - i = pid->integral; // If saturated then do not update integral value - pid->saturated = 1; - - } else { - if (!isfinite(i)) { - i = 0.0f; + /* check for saturation */ + if (isfinite(i)) { + if ((pid->output_limit < SIGMA || (fabsf(output + (i * pid->ki)) <= pid->output_limit)) && + fabsf(i) <= pid->integral_limit) { + /* not saturated, use new integral value */ + pid->integral = i; } - - pid->integral = i; - pid->saturated = 0; } - } else { - i = 0.0f; - pid->saturated = 0; + /* add I component to output */ + output += pid->integral * pid->ki; } - // Calculate the output. Limit output magnitude to pid->limit - float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); - + /* limit output */ if (isfinite(output)) { - if (pid->limit > SIGMA) { - if (output > pid->limit) { - output = pid->limit; + if (pid->output_limit > SIGMA) { + if (output > pid->output_limit) { + output = pid->output_limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->output_limit) { + output = -pid->output_limit; } } @@ -212,5 +180,5 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo __EXPORT void pid_reset_integral(PID_t *pid) { - pid->integral = 0; + pid->integral = 0.0f; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index eca228464..e8b1aac4f 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -39,7 +39,7 @@ /** * @file pid.h * - * Definition of generic PID control interface. + * Definition of generic PID controller. * * @author Laurens Mackay <mackayl@student.ethz.ch> * @author Tobias Naegeli <naegelit@student.ethz.ch> @@ -55,38 +55,35 @@ __BEGIN_DECLS -/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC 0 -/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored - * val_dot in pid_calculate() will be ignored */ -#define PID_MODE_DERIVATIV_CALC_NO_SP 1 -/* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 2 -// Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) -#define PID_MODE_DERIVATIV_NONE 9 +typedef enum PID_MODE { + /* Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) */ + PID_MODE_DERIVATIV_NONE = 0, + /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error, + * val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC, + /* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, + * setpoint derivative will be ignored, val_dot in pid_calculate() will be ignored */ + PID_MODE_DERIVATIV_CALC_NO_SP, + /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ + PID_MODE_DERIVATIV_SET +} pid_mode_t; typedef struct { + pid_mode_t mode; + float dt_min; float kp; float ki; float kd; - float intmax; - float sp; float integral; + float integral_limit; + float output_limit; float error_previous; float last_output; - float limit; - float dt_min; - uint8_t mode; - uint8_t count; - uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); -//void pid_set(PID_t *pid, float sp); +__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float integral_limit, float output_limit); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); - __EXPORT void pid_reset_integral(PID_t *pid); __END_DECLS 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 |