Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

added filter option

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@969 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-11-28 07:03:23 +00:00
parent 7a0fcd6f59
commit 44f2139951
3 changed files with 167 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
libraries/RC_Channel/RC_Channel.cpp
View File

@ -9,15 +9,14 @@
*/
#include <math.h>
#include <avr/eeprom.h>
#include "WProgram.h"
#include "RC_Channel.h"
#define ANGLE 0
#define RANGE 1
RC_Channel::RC_Channel() : _high(1)
{
}
// setup the control preferences
void
RC_Channel::set_range(int low, int high)
@ -34,6 +33,12 @@ RC_Channel::set_angle(int angle)
_high = angle;
}
void
RC_Channel::set_filter(bool filter)
{
_filter = filter;
}
// call after first read
void
RC_Channel::trim()
@ -48,11 +53,15 @@ RC_Channel::set_pwm(int pwm)
{
//Serial.print(pwm,DEC);
if(radio_in == 0)
if(_filter){
if(radio_in == 0)
radio_in = pwm;
else
radio_in = ((pwm + radio_in) >> 1); // Small filtering
}else{
radio_in = pwm;
else
radio_in = ((pwm + radio_in) >> 1); // Small filtering
}
if(_type == RANGE){
//Serial.print("range ");
control_in = pwm_to_range();
@ -70,7 +79,7 @@ RC_Channel::control_mix(float value)
}
// are we below a threshold?
boolean
bool
RC_Channel::get_failsafe(void)
{
return (radio_in < (radio_min - 50));
@ -90,28 +99,30 @@ RC_Channel::calc_pwm(void)
}
void
RC_Channel::load_eeprom(int address)
RC_Channel::load_eeprom(void)
{
radio_min = eeprom_read_word((uint16_t *) address);
radio_max = eeprom_read_word((uint16_t *) (address + 2));
radio_trim = eeprom_read_word((uint16_t *) (address + 4));
radio_min = eeprom_read_word((uint16_t *) _address);
radio_max = eeprom_read_word((uint16_t *) (_address + 2));
radio_trim = eeprom_read_word((uint16_t *) (_address + 4));
}
void
RC_Channel::save_eeprom(int address)
RC_Channel::save_eeprom(void)
{
eeprom_write_word((uint16_t *) address, radio_min);
eeprom_write_word((uint16_t *) (address + 2), radio_max);
eeprom_write_word((uint16_t *) (address + 4), radio_trim);
eeprom_write_word((uint16_t *) _address, radio_min);
eeprom_write_word((uint16_t *) (_address + 2), radio_max);
eeprom_write_word((uint16_t *) (_address + 4), radio_trim);
}
// ------------------------------------------
void
RC_Channel::update_min_max()
{
radio_min = min(radio_min, radio_in);
radio_max = min(radio_max, radio_in);
radio_max = max(radio_max, radio_in);
}
// ------------------------------------------
int16_t

60
libraries/RC_Channel/RC_Channel.h
View File

@ -1,27 +1,44 @@
/*
RC_Channel library.
Code by Jason Short. 2010
DIYDrones.com
*/
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file RC_Channel.h
/// @brief RC_Channel manager, with EEPROM-backed storage of constants.
#ifndef RC_Channel_h
#define RC_Channel_h
#include <inttypes.h>
#include <avr/eeprom.h>
#include "WProgram.h"
#include <stdint.h>
class RC_Channel
{
public:
RC_Channel();
// setup in CLI
/// @class RC_Channel
/// @brief Object managing one RC channel
class RC_Channel{
public:
/// Constructor
///
/// A RC_Channel constructed in this fashion does not support save/restore.
///
RC_Channel() :
_address(0)
{}
/// Constructor
///
/// @param address EEPROM base address at which RC_Channel parameters
/// are stored. Zero if the RC_Channel does not support
/// save/restore.
///
RC_Channel(uint16_t address) :
_address(address),
_high(1),
_filter(true)
{}
// setup min and max radio values in CLI
void update_min_max();
// startup
void load_eeprom(int address);
void save_eeprom(int address);
void load_eeprom(void);
void save_eeprom(void);
void set_filter(bool filter);
// setup the control preferences
void set_range(int low, int high);
@ -37,7 +54,7 @@ class RC_Channel
void trim();
// did our read come in 50µs below the min?
boolean get_failsafe(void);
bool get_failsafe(void);
// value generated from PWM
int16_t control_in;
@ -63,17 +80,14 @@ class RC_Channel
//int16_t get_radio_out(void);
private:
int16_t channel_filter();
bool _filter;
int16_t pwm_to_angle();
int16_t angle_to_pwm();
int16_t pwm_to_range();
int16_t range_to_pwm();
byte _channel;
boolean _type;
boolean _filter;
int16_t _address; ///< EEPROM address for save/restore of P/I/D
bool _type;
int16_t _high;
int16_t _low;
};

109
libraries/RC_Channel/examples/RC_Channel/RC_Channel.pde
View File

@ -8,15 +8,34 @@
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <RC_Channel.h> // ArduPilot Mega RC Library
RC_Channel rc_1;
RC_Channel rc_2;
RC_Channel rc_3;
RC_Channel rc_4;
#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
#define EE_RADIO_5 0x18 // all gains stored from here
#define EE_RADIO_6 0x1E // all gains stored from here
#define EE_RADIO_7 0x24 // all gains stored from here
#define EE_RADIO_8 0x2A // all gains stored from here
RC_Channel rc_1(EE_RADIO_1);
RC_Channel rc_2(EE_RADIO_2);
RC_Channel rc_3(EE_RADIO_3);
RC_Channel rc_4(EE_RADIO_4);
RC_Channel rc_5(EE_RADIO_5);
RC_Channel rc_6(EE_RADIO_6);
RC_Channel rc_7(EE_RADIO_7);
RC_Channel rc_8(EE_RADIO_8);
#define CH_1 0
#define CH_2 1
#define CH_3 2
#define CH_4 3
#define CH_5 4
#define CH_6 5
#define CH_7 6
#define CH_8 7
void setup()
{
@ -28,6 +47,7 @@ void setup()
// setup radio
// read eepom or set manually
/*
rc_1.radio_min = 1100;
rc_1.radio_max = 1900;
@ -39,23 +59,40 @@ void setup()
rc_4.radio_min = 1100;
rc_4.radio_max = 1900;
*/
rc_1.load_eeprom();
rc_2.load_eeprom();
rc_3.load_eeprom();
rc_4.load_eeprom();
rc_5.load_eeprom();
rc_6.load_eeprom();
rc_7.load_eeprom();
rc_8.load_eeprom();
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(3000);
rc_5.set_range(0,1000);
rc_6.set_range(0,1000);
rc_7.set_range(0,1000);
rc_8.set_range(0,1000);
// set midpoint value
rc_1.set_pwm(APM_RC.InputCh(CH_1));
rc_2.set_pwm(APM_RC.InputCh(CH_2));
rc_3.set_pwm(APM_RC.InputCh(CH_3));
rc_4.set_pwm(APM_RC.InputCh(CH_4));
rc_1.trim();
rc_2.trim();
rc_3.trim();
rc_4.trim();
}
void loop()
@ -65,6 +102,10 @@ void loop()
rc_2.set_pwm(APM_RC.InputCh(CH_2));
rc_3.set_pwm(APM_RC.InputCh(CH_3));
rc_4.set_pwm(APM_RC.InputCh(CH_4));
rc_5.set_pwm(APM_RC.InputCh(CH_5));
rc_6.set_pwm(APM_RC.InputCh(CH_6));
rc_7.set_pwm(APM_RC.InputCh(CH_7));
rc_8.set_pwm(APM_RC.InputCh(CH_8));
print_pwm();
}
@ -78,5 +119,57 @@ void print_pwm()
Serial.print("\tch3 :");
Serial.print(rc_3.control_in, DEC);
Serial.print("\tch4 :");
Serial.println(rc_4.control_in, DEC);
}
Serial.print(rc_4.control_in, DEC);
Serial.print("\tch5 :");
Serial.print(rc_5.control_in, DEC);
Serial.print("\tch6 :");
Serial.print(rc_6.control_in, DEC);
Serial.print("\tch7 :");
Serial.print(rc_7.control_in, DEC);
Serial.print("\tch8 :");
Serial.println(rc_8.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);
Serial.print("CH5: ");
Serial.print(rc_5.radio_min, DEC);
Serial.print(" | ");
Serial.println(rc_5.radio_max, DEC);
Serial.print("CH6: ");
Serial.print(rc_6.radio_min, DEC);
Serial.print(" | ");
Serial.println(rc_6.radio_max, DEC);
Serial.print("CH7: ");
Serial.print(rc_7.radio_min, DEC);
Serial.print(" | ");
Serial.println(rc_7.radio_max, DEC);
Serial.print("CH8: ");
Serial.print(rc_8.radio_min, DEC);
Serial.print(" | ");
Serial.println(rc_8.radio_max, DEC);
}