Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

added servo_in 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@401 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2010-09-06 06:14:24 +00:00
parent 4b762bf933
commit 20987c1b89
4 changed files with 66 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/RC/APM2_RC.cpp
View File

@ -154,9 +154,34 @@ void APM2_RC::read_pwm()
//Serial.println(radio_in[CH2],DEC);
}
// output servo
servo_out[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
servo_out[CH3] = constrain(servo_out[CH3], 0, 100);
// output servos
for (uint8_t i = 0; i < 8; i++){
if (i == 3) continue;
if(radio_in[i] >= radio_trim[i])
servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0;
else
servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0;
}
servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
servo_in[CH3] = constrain(servo_out[CH3], 0, 100);
}
void
APM2_RC::output()
{
uint16_t out;
for (uint8_t i = 0; i < 8; i++){
if (i == 3) continue;
if(radio_in[i] >= radio_trim[i])
out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
else
out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
set_ch_pwm(i, out);
}
out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
out += radio_min[CH3];
set_ch_pwm(CH3, out);
}
void
@ -173,14 +198,6 @@ APM2_RC::trim()
radio_trim[y] = radio_in[y];
}
void
APM2_RC::set_throttle(float percent)
{
uint16_t out = (percent * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
out += radio_min[CH3];
set_ch_pwm(CH3, out);
}
void
APM2_RC::set_ch_pwm(uint8_t ch, uint16_t pwm)
{

7
libraries/RC/APM2_RC.h
View File

@ -9,10 +9,10 @@ class APM2_RC : public RC
{
public:
APM2_RC();
void read_pwm();
void set_ch_pwm(uint8_t ch, uint16_t pwm);
void init();
void set_throttle(float percent);
void read_pwm();
void output();
void set_ch_pwm(uint8_t ch, uint16_t pwm);
void trim();
void force_out_0_1(void);
@ -24,6 +24,7 @@ class APM2_RC : public RC
int16_t radio_trim[8];
int16_t radio_max[8];
int16_t servo_in[8];
float servo_out[8];
private:

38
libraries/RC/AP_RC.cpp
View File

@ -128,9 +128,34 @@ AP_RC::read_pwm()
check_throttle_failsafe(radio_in[CH3]);
// output servo
servo_out[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
servo_out[CH3] = constrain(servo_out[CH3], 0, 100);
// output servos
for (uint8_t i = 0; i < 4; i++){
if (i == 3) continue;
if(radio_in[i] >= radio_trim[i])
servo_in[i] = (float)(radio_in[i] - radio_min[i]) / (float)(radio_max[i] - radio_min[i]) * 100.0;
else
servo_in[i] = (float)(radio_in[i] - radio_trim[i]) / (float)(radio_trim[i] - radio_min[i]) * 100.0;
}
servo_in[CH3] = (float)(radio_in[CH3] - radio_min[CH3]) / (float)(radio_max[CH3] - radio_min[CH3]) * 100.0;
servo_in[CH3] = constrain(servo_out[CH3], 0, 100);
}
void
AP_RC::output()
{
uint16_t out;
for (uint8_t i = 0; i < 4; i++){
if (i == 3) continue;
if(radio_in[i] >= radio_trim[i])
out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
else
out = ((servo_in[i] * (radio_max[i] - radio_trim[i])) / 100) + radio_trim[i];
set_ch_pwm(i, out);
}
out = (servo_out[CH3] * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
out += radio_min[CH3];
set_ch_pwm(CH3, out);
}
void
@ -150,14 +175,11 @@ AP_RC::trim()
//Serial.println(radio_trim[CH1], DEC);
}
void
/*void
AP_RC::set_throttle(float percent)
{
uint16_t out = (percent * (float)(radio_max[CH3] - radio_min[CH3])) / 100.0;
out += radio_min[CH3];
set_ch_pwm(CH3, out);
}
*/
void
AP_RC::set_ch_pwm(uint8_t ch, uint16_t pwm)
{

7
libraries/RC/AP_RC.h
View File

@ -9,10 +9,10 @@ class AP_RC : public RC
{
public:
AP_RC();
void read_pwm();
void set_ch_pwm(uint8_t ch, uint16_t pwm);
void init();
void set_throttle(float percent);
void read_pwm();
void output();
void set_ch_pwm(uint8_t ch, uint16_t pwm);
void trim();
int16_t radio_in[4];
@ -20,6 +20,7 @@ class AP_RC : public RC
int16_t radio_trim[4];
int16_t radio_max[4];
int16_t servo_in[4];
float servo_out[4];
private: