mirror of https://github.com/ArduPilot/ardupilot
Libraries to support non AP_VAR usage. This is for Ardupilot legacy hardware.
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2075 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
5a6b9adcca
commit
fc4759dfbb
|
|
@ -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 <math.h>
|
||||||
|
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
@ -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 <AP_Common.h>
|
||||||
|
#include <math.h> // 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
|
||||||
|
|
@ -0,0 +1,48 @@
|
||||||
|
/*
|
||||||
|
Example of PID library.
|
||||||
|
2010 Code by Jason Short. DIYDrones.com
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <FastSerial.h>
|
||||||
|
#include <AP_Common.h>
|
||||||
|
#include <APM_RC.h> // ArduPilot RC Library
|
||||||
|
#include <PID.h> // 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);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,2 @@
|
||||||
|
BOARD = mega
|
||||||
|
include ../../../AP_Common/Arduino.mk
|
||||||
|
|
@ -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
|
||||||
|
|
@ -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 <math.h>
|
||||||
|
#include <avr/eeprom.h>
|
||||||
|
#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);
|
||||||
|
}
|
||||||
|
|
@ -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 <AP_Common.h>
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
/// @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
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -0,0 +1,223 @@
|
||||||
|
/*
|
||||||
|
Example of AP_RC_Channel library.
|
||||||
|
Code by Jason Short. 2010
|
||||||
|
DIYDrones.com
|
||||||
|
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <AP_RC.h> // ArduPilot RC Library
|
||||||
|
#include <AP_RC_Channel.h> // 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;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,2 @@
|
||||||
|
BOARD = mega
|
||||||
|
include ../../../AP_Common/Arduino.mk
|
||||||
Loading…
Reference in New Issue