diff --git a/libraries/AP_PID/AP_PID.cpp b/libraries/AP_PID/AP_PID.cpp new file mode 100644 index 0000000000..ed3541561b --- /dev/null +++ b/libraries/AP_PID/AP_PID.cpp @@ -0,0 +1,65 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_PID.cpp +/// @brief Generic PID algorithm + +#include + +#include "AP_PID.h" + +AP_PID::AP_PID() +{ +} + +long +AP_PID::get_pid(int32_t error, uint16_t dt, float scaler) +{ + float output = 0; + float delta_time = (float)dt / 1000.0; + + // Compute proportional component + output += error * _kp; + + // Compute derivative component if time has elapsed + if ((fabs(_kd) > 0) && (dt > 0)) { + float derivative = (error - _last_error) / delta_time; + + // discrete low pass filter, cuts out the + // high frequency noise that can drive the controller crazy + float RC = 1/(2*M_PI*_fCut); + derivative = _last_derivative + + (delta_time / (RC + delta_time)) * (derivative - _last_derivative); + + // update state + _last_error = error; + _last_derivative = derivative; + + // add in derivative component + output += _kd * derivative; + } + + // scale the P and D components + output *= scaler; + + // Compute integral component if time has elapsed + if ((fabs(_ki) > 0) && (dt > 0)) { + _integrator += (error * _ki) * scaler * delta_time; + if (_integrator < -_imax) { + _integrator = -_imax; + } else if (_integrator > _imax) { + _integrator = _imax; + } + output += _integrator; + } + + return output; +} + +void +AP_PID::reset_I() +{ + _integrator = 0; + _last_error = 0; + _last_derivative = 0; +} + diff --git a/libraries/AP_PID/AP_PID.h b/libraries/AP_PID/AP_PID.h new file mode 100644 index 0000000000..cbddea4958 --- /dev/null +++ b/libraries/AP_PID/AP_PID.h @@ -0,0 +1,55 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_PID.h +/// @brief Generic PID algorithm, with EEPROM-backed storage of constants. + +#ifndef AP_PID_h +#define AP_PID_h + +#include +#include // for fabs() + +/// @class AP_PID +/// @brief Object managing one PID control +class AP_PID { +public: + + AP_PID(); + + long get_pid(int32_t error, uint16_t dt, float scaler = 1.0); + + /// Reset the PID integrator + /// + void reset_I(); + + void kP(const float v) { _kp = v; } + void kI(const float v) { _ki = v; } + void kD(const float v) { _kd = v; } + void imax(const int16_t v) { _imax = v; } + + float kP() { return _kp; } + float kI() { return _ki; } + float kD() { return _kd; } + float imax() { return _imax; } + + float get_integrator() const { return _integrator; } + +private: + float _kp; + float _ki; + float _kd; + float _imax; + + float _integrator; ///< integrator value + int32_t _last_error; ///< last error for derivative + float _last_derivative; ///< last derivative for low-pass filter + + /// Low pass filter cut frequency for derivative calculation. + /// + /// 20 Hz becasue anything over that is probably noise, see + /// http://en.wikipedia.org/wiki/Low-pass_filter. + /// + static const uint8_t _fCut = 20; +}; + +#endif diff --git a/libraries/AP_PID/examples/AP_pid/AP_pid.pde b/libraries/AP_PID/examples/AP_pid/AP_pid.pde new file mode 100644 index 0000000000..ca46f3d6db --- /dev/null +++ b/libraries/AP_PID/examples/AP_pid/AP_pid.pde @@ -0,0 +1,48 @@ +/* + Example of PID library. + 2010 Code by Jason Short. DIYDrones.com +*/ + +#include +#include +#include // ArduPilot RC Library +#include // ArduPilot Mega RC Library + +long radio_in; +long radio_trim; + +PID pid(); + +void setup() +{ + Serial.begin(38400); + Serial.println("ArduPilot Mega PID library test"); + APM_RC.Init(); // APM Radio initialization + + delay(1000); + //rc.trim(); + radio_trim = APM_RC.InputCh(0); + + pid.kP(1); + pid.kI(0); + pid.kD(0.5); + pid.imax(50); + pid.save_gains(); + pid.kP(0); + pid.kI(0); + pid.kD(0); + pid.imax(0); + pid.load_gains(); + Serial.printf("P %f I %f D %f imax %f\n", pid.kP(), pid.kI(), pid.kD(), pid.imax()); +} + +void loop() +{ + delay(20); + //rc.read_pwm(); + long error = APM_RC.InputCh(0) - radio_trim; + long control = pid.get_pid(error, 20, 1); + + Serial.print("control: "); + Serial.println(control,DEC); +} diff --git a/libraries/AP_PID/examples/AP_pid/Makefile b/libraries/AP_PID/examples/AP_pid/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/AP_PID/examples/AP_pid/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk diff --git a/libraries/AP_PID/keywords.txt b/libraries/AP_PID/keywords.txt new file mode 100644 index 0000000000..68e861fd00 --- /dev/null +++ b/libraries/AP_PID/keywords.txt @@ -0,0 +1,9 @@ +PID KEYWORD1 +get_pid KEYWORD2 +reset_I KEYWORD2 +kP KEYWORD2 +kD KEYWORD2 +kI KEYWORD2 +imax KEYWORD2 +load_gains KEYWORD2 +save_gains KEYWORD2 diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.cpp b/libraries/AP_RC_Channel/AP_RC_Channel.cpp new file mode 100644 index 0000000000..288f9c7353 --- /dev/null +++ b/libraries/AP_RC_Channel/AP_RC_Channel.cpp @@ -0,0 +1,240 @@ +/* + AP_RC_Channel.cpp - Radio library for Arduino + Code by Jason Short. DIYDrones.com + + This library is free software; you can redistribute it and / or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + +*/ + +#include +#include +#include "WProgram.h" +#include "AP_RC_Channel.h" + +#define ANGLE 0 +#define RANGE 1 + +// setup the control preferences +void +AP_RC_Channel::set_range(int low, int high) +{ + _type = RANGE; + _high = high; + _low = low; +} + +void +AP_RC_Channel::set_angle(int angle) +{ + _type = ANGLE; + _high = angle; +} + +void +AP_RC_Channel::set_reverse(bool reverse) +{ + if (reverse) _reverse = -1; + else _reverse = 1; +} + +bool +AP_RC_Channel::get_reverse(void) +{ + if (_reverse==-1) return 1; + else return 0; +} + +void +AP_RC_Channel::set_filter(bool filter) +{ + _filter = filter; +} + +// call after first read +void +AP_RC_Channel::trim() +{ + radio_trim = radio_in; +} + +// read input from APM_RC - create a control_in value +void +AP_RC_Channel::set_pwm(int pwm) +{ + //Serial.print(pwm,DEC); + + if(_filter){ + if(radio_in == 0) + radio_in = pwm; + else + radio_in = ((pwm + radio_in) >> 1); // Small filtering + }else{ + radio_in = pwm; + } + + if(_type == RANGE){ + //Serial.print("range "); + control_in = pwm_to_range(); + control_in = (control_in < dead_zone) ? 0 : control_in; + if (fabs(scale_output) > 0){ + control_in *= scale_output; + } + + }else{ + control_in = pwm_to_angle(); + control_in = (abs(control_in) < dead_zone) ? 0 : control_in; + if (fabs(scale_output) > 0){ + control_in *= scale_output; + } + } +} + +int +AP_RC_Channel::control_mix(float value) +{ + return (1 - abs(control_in / _high)) * value + control_in; +} + +// are we below a threshold? +bool +AP_RC_Channel::get_failsafe(void) +{ + return (radio_in < (radio_min - 50)); +} + +// returns just the PWM without the offset from radio_min +void +AP_RC_Channel::calc_pwm(void) +{ + + if(_type == RANGE){ + pwm_out = range_to_pwm(); + radio_out = pwm_out + radio_min; + }else{ + pwm_out = angle_to_pwm(); + radio_out = pwm_out + radio_trim; + } + radio_out = constrain(radio_out, radio_min, radio_max); +} + +// ------------------------------------------ + +void +AP_RC_Channel::load_eeprom(void) +{ + radio_min = eeprom_read_word((uint16_t *) _address); + radio_max = eeprom_read_word((uint16_t *) (_address + 2)); + load_trim(); +} + +void +AP_RC_Channel::save_eeprom(void) +{ + eeprom_write_word((uint16_t *) _address, radio_min); + eeprom_write_word((uint16_t *) (_address + 2), radio_max); + save_trim(); +} + +// ------------------------------------------ +void +AP_RC_Channel::save_trim(void) +{ + eeprom_write_word((uint16_t *) (_address + 4), radio_trim); + //_ee.write_int((_address + 4), radio_trim); +} + +void +AP_RC_Channel::load_trim(void) +{ + radio_trim = eeprom_read_word((uint16_t *) (_address + 4)); + //_ee.write_int((_address + 4), radio_trim); +} + +// ------------------------------------------ + +void +AP_RC_Channel::zero_min_max() +{ + radio_min = radio_max = radio_in; +} + +void +AP_RC_Channel::update_min_max() +{ + radio_min = min(radio_min, radio_in); + radio_max = max(radio_max, radio_in); +} + +// ------------------------------------------ + +int16_t +AP_RC_Channel::pwm_to_angle() +{ + if(radio_in < radio_trim) + return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_trim - radio_min); + else + return _reverse * ((long)_high * (long)(radio_in - radio_trim)) / (long)(radio_max - radio_trim); + + //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_max - radio_trim)); + //return _reverse * _high * ((float)(radio_in - radio_trim) / (float)(radio_trim - radio_min)); +} + + +int16_t +AP_RC_Channel::angle_to_pwm() +{ + if(_reverse == -1) + { + if(servo_out < 0) + return ( -1 * ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high); + else + return ( -1 * ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high); + } else { + if(servo_out > 0) + return ((long)servo_out * (long)(radio_max - radio_trim)) / (long)_high; + else + return ((long)servo_out * (long)(radio_trim - radio_min)) / (long)_high; + } + + //return (((float)servo_out / (float)_high) * (float)(radio_max - radio_trim)); + //return (((float)servo_out / (float)_high) * (float)(radio_trim - radio_min)); +} + +// ------------------------------------------ + +int16_t +AP_RC_Channel::pwm_to_range() +{ + //return (_low + ((_high - _low) * ((float)(radio_in - radio_min) / (float)(radio_max - radio_min)))); + return (_low + ((long)(_high - _low) * (long)(radio_in - radio_min)) / (long)(radio_max - radio_min)); +} + +int16_t +AP_RC_Channel::range_to_pwm() +{ + //return (((float)(servo_out - _low) / (float)(_high - _low)) * (float)(radio_max - radio_min)); + return ((long)(servo_out - _low) * (long)(radio_max - radio_min)) / (long)(_high - _low); +} + +// ------------------------------------------ + +float +AP_RC_Channel::norm_input() +{ + if(radio_in < radio_trim) + return _reverse * (float)(radio_in - radio_trim) / (float)(radio_trim - radio_min); + else + return _reverse * (float)(radio_in - radio_trim) / (float)(radio_max - radio_trim); +} + +float +AP_RC_Channel::norm_output() +{ + if(radio_out < radio_trim) + return (float)(radio_out - radio_trim) / (float)(radio_trim - radio_min); + else + return (float)(radio_out - radio_trim) / (float)(radio_max - radio_trim); +} diff --git a/libraries/AP_RC_Channel/AP_RC_Channel.h b/libraries/AP_RC_Channel/AP_RC_Channel.h new file mode 100644 index 0000000000..d39806d904 --- /dev/null +++ b/libraries/AP_RC_Channel/AP_RC_Channel.h @@ -0,0 +1,112 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file AP_RC_Channel.h +/// @brief AP_RC_Channel manager, with EEPROM-backed storage of constants. + +#ifndef AP_RC_Channel_h +#define AP_RC_Channel_h + +//#include +#include + +/// @class AP_RC_Channel +/// @brief Object managing one RC channel +class AP_RC_Channel{ + public: + /// Constructor + /// + /// @param key EEPROM storage key for the channel trim parameters. + /// @param name Optional name for the group. + /// + AP_RC_Channel(uint16_t address) : + _address(address), + _high(1), + _filter(true), + _reverse(1), + dead_zone(0), + scale_output(1.0) + {} + + AP_RC_Channel() : + _high(1), + _filter(true), + _reverse(1), + dead_zone(0), + scale_output(1.0) + {} + + // setup min and max radio values in CLI + void update_min_max(); + void zero_min_max(); + + // startup + void load_eeprom(void); + void save_eeprom(void); + void save_trim(void); + void load_trim(void); + void set_filter(bool filter); + + // setup the control preferences + void set_range(int low, int high); + void set_angle(int angle); + void set_reverse(bool reverse); + bool get_reverse(void); + + // read input from APM_RC - create a control_in value + void set_pwm(int pwm); + + // pwm is stored here + int16_t radio_in; + + // call after first set_pwm + void trim(); + + // did our read come in 50µs below the min? + bool get_failsafe(void); + + // value generated from PWM + int16_t control_in; + int16_t dead_zone; // used to keep noise down and create a dead zone. + + int control_mix(float value); + + // current values to the servos - degrees * 100 (approx assuming servo is -45 to 45 degrees except [3] is 0 to 100 + int16_t servo_out; + + // generate PWM from servo_out value + void calc_pwm(void); + + // PWM is without the offset from radio_min + int16_t pwm_out; + int16_t radio_out; + + int16_t radio_min; + int16_t radio_trim; + int16_t radio_max; + + // includes offset from PWM + //int16_t get_radio_out(void); + + int16_t pwm_to_angle(); + float norm_input(); + float norm_output(); + int16_t angle_to_pwm(); + int16_t pwm_to_range(); + int16_t range_to_pwm(); + + float scale_output; + + private: + bool _filter; + int8_t _reverse; + int16_t _address; + bool _type; + int16_t _high; + int16_t _low; +}; + +#endif + + + + diff --git a/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde b/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde new file mode 100644 index 0000000000..f636c37aa1 --- /dev/null +++ b/libraries/AP_RC_Channel/examples/AP_RC_Channel/AP_RC_Channel.pde @@ -0,0 +1,223 @@ +/* + Example of AP_RC_Channel library. + Code by Jason Short. 2010 + DIYDrones.com + +*/ + +#include // ArduPilot RC Library +#include // ArduPilot RC Library + + +#define EE_RADIO_1 0x00 // all gains stored from here +#define EE_RADIO_2 0x06 // all gains stored from here +#define EE_RADIO_3 0x0C // all gains stored from here +#define EE_RADIO_4 0x12 // all gains stored from here + +AP_RC_Channel rc_1(EE_RADIO_1); +AP_RC_Channel rc_2(EE_RADIO_2); +AP_RC_Channel rc_3(EE_RADIO_3); +AP_RC_Channel rc_4(EE_RADIO_4); + +AP_RC rc; + +#define CH_1 0 +#define CH_2 1 +#define CH_3 2 +#define CH_4 3 + +void setup() +{ + Serial.begin(115200); + //Serial.begin(38400); + + Serial.println("ArduPilot RC Channel test"); + rc.init(); // APM Radio initialization + + + delay(500); + + // setup radio + + // read eepom or set manually + /* + rc_1.radio_min = 1100; + rc_1.radio_max = 1900; + + rc_2.radio_min = 1100; + rc_2.radio_max = 1900; + + rc_3.radio_min = 1100; + rc_3.radio_max = 1900; + + rc_4.radio_min = 1100; + rc_4.radio_max = 1900; + + // or + + rc_1.load_eeprom(); + rc_2.load_eeprom(); + rc_3.load_eeprom(); + rc_4.load_eeprom(); + + */ + + // interactive setup + setup_radio(); + + print_radio_values(); + + // set type of output, symmetrical angles or a number range; + rc_1.set_angle(4500); + rc_2.set_angle(4500); + rc_3.set_range(0,1000); + rc_4.set_angle(4500); + + rc_1.dead_zone = 85; + rc_2.dead_zone = 85; + rc_3.dead_zone = 85; + rc_4.dead_zone = 85; + + for (byte i = 0; i < 30; i++){ + read_radio(); + } + rc_1.trim(); + rc_2.trim(); + rc_4.trim(); +} + +void loop() +{ + delay(20); + read_radio(); + rc_1.servo_out = rc_1.control_in; + rc_2.servo_out = rc_2.control_in; + rc_3.servo_out = rc_3.control_in; + rc_4.servo_out = rc_4.control_in; + + rc_1.calc_pwm(); + rc_2.calc_pwm(); + rc_3.calc_pwm(); + rc_4.calc_pwm(); + + print_pwm(); + print_control_in(); + + // send values to the PWM timers for output + // ---------------------------------------- + rc.output_ch_pwm(CH_1, rc_1.radio_out); // send to Servos + rc.output_ch_pwm(CH_2, rc_2.radio_out); // send to Servos + rc.output_ch_pwm(CH_3, rc_3.radio_out); // send to Servos + rc.output_ch_pwm(CH_4, rc_4.radio_out); // send to Servos +} + + +void read_radio() +{ + rc_1.set_pwm(rc.input_ch(CH_1)); + rc_2.set_pwm(rc.input_ch(CH_2)); + rc_3.set_pwm(rc.input_ch(CH_3)); + rc_4.set_pwm(rc.input_ch(CH_4)); + //Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"), rc_1.control_in, rc_2.control_in, rc_3.control_in, rc_4.control_in); +} + +void print_pwm() +{ + Serial.print("1: "); + Serial.print(rc_1.radio_out, DEC); + Serial.print("\t2: "); + Serial.print(rc_2.radio_out, DEC); + Serial.print("\t3:"); + Serial.print(rc_3.radio_out, DEC); + Serial.print("\t4:"); + Serial.print(rc_4.radio_out , DEC); +} + +// 1280 +// 1536 +// 1795 +void print_control_in() +{ + Serial.print("\t1: "); + Serial.print(rc_1.control_in, DEC); + Serial.print("\t2: "); + Serial.print(rc_2.control_in, DEC); + Serial.print("\t3:"); + Serial.print(rc_3.control_in, DEC); + Serial.print("\t4:"); + Serial.println(rc_4.control_in, DEC); +} + +void +print_radio_values() +{ + Serial.print("CH1: "); + Serial.print(rc_1.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_1.radio_max, DEC); + + Serial.print("CH2: "); + Serial.print(rc_2.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_2.radio_max, DEC); + + Serial.print("CH3: "); + Serial.print(rc_3.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_3.radio_max, DEC); + + Serial.print("CH4: "); + Serial.print(rc_4.radio_min, DEC); + Serial.print(" | "); + Serial.println(rc_4.radio_max, DEC); +} + + +void +setup_radio() +{ + Serial.println("\n\nRadio Setup:"); + uint8_t i; + + for(i = 0; i < 100;i++){ + delay(20); + read_radio(); + } + + rc_1.radio_min = rc_1.radio_in; + rc_2.radio_min = rc_2.radio_in; + rc_3.radio_min = rc_3.radio_in; + rc_4.radio_min = rc_4.radio_in; + + rc_1.radio_max = rc_1.radio_in; + rc_2.radio_max = rc_2.radio_in; + rc_3.radio_max = rc_3.radio_in; + rc_4.radio_max = rc_4.radio_in; + + rc_1.radio_trim = rc_1.radio_in; + rc_2.radio_trim = rc_2.radio_in; + rc_4.radio_trim = rc_4.radio_in; + + Serial.println("\nMove all controls to each extreme. Hit Enter to save:"); + while(1){ + + delay(20); + // Filters radio input - adjust filters in the radio.pde file + // ---------------------------------------------------------- + read_radio(); + + rc_1.update_min_max(); + rc_2.update_min_max(); + rc_3.update_min_max(); + rc_4.update_min_max(); + + if(Serial.available() > 0){ + //rc_3.radio_max += 250; + Serial.flush(); + + Serial.println("Radio calibrated, Showing control values:"); + break; + } + } + return; +} \ No newline at end of file diff --git a/libraries/AP_RC_Channel/examples/AP_RC_Channel/Makefile b/libraries/AP_RC_Channel/examples/AP_RC_Channel/Makefile new file mode 100644 index 0000000000..85b4d8856b --- /dev/null +++ b/libraries/AP_RC_Channel/examples/AP_RC_Channel/Makefile @@ -0,0 +1,2 @@ +BOARD = mega +include ../../../AP_Common/Arduino.mk