Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-12 01:23:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Rover: support RCMAP_* mapping for steering/throttle

Browse Source
This commit is contained in:
Andrew Tridgell 2013-06-03 19:33:59 +10:00
parent 5b3bfe3d38
commit 5a9485a419
13 changed files with 125 additions and 137 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
APMrover2/APMrover2.pde
View File

@ -73,6 +73,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_Airspeed.h> // needed for AHRS build #include <AP_Airspeed.h> // needed for AHRS build
#include <memcheck.h> #include <memcheck.h>
#include <DataFlash.h> #include <DataFlash.h>
#include <AP_RCMapper.h> // RC input mapping library
#include <SITL.h> #include <SITL.h>
#include <stdarg.h> #include <stdarg.h>
@ -115,6 +116,12 @@ static const AP_InertialSensor::Sample_rate ins_sample_rate = AP_InertialSensor:
// //
static Parameters g; static Parameters g;
// mapping between input channels
static RCMapper rcmap;
// primary control channels
static RC_Channel *channel_steer;
static RC_Channel *channel_throttle;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// prototypes // prototypes
@ -795,6 +802,8 @@ static void one_second_loop()
// allow orientation change at runtime to aid config // allow orientation change at runtime to aid config
ahrs.set_orientation(); ahrs.set_orientation();
set_control_channels();
} }
static void update_GPS(void) static void update_GPS(void)
@ -855,12 +864,12 @@ static void update_current_mode(void)
the same type of steering control as auto mode. The throttle the same type of steering control as auto mode. The throttle
controls the target speed, in proportion to the throttle controls the target speed, in proportion to the throttle
*/ */
bearing_error_cd = g.channel_steer.pwm_to_angle(); bearing_error_cd = channel_steer->pwm_to_angle();
calc_nav_steer(); calc_nav_steer();
/* we need to reset the I term or it will build up */ /* we need to reset the I term or it will build up */
g.pidNavSteer.reset_I(); g.pidNavSteer.reset_I();
calc_throttle(g.channel_throttle.pwm_to_angle() * 0.01 * g.speed_cruise); calc_throttle(channel_throttle->pwm_to_angle() * 0.01 * g.speed_cruise);
break; break;
case LEARNING: case LEARNING:
@ -871,14 +880,14 @@ static void update_current_mode(void)
we set the exact value in set_servos(), but it helps for we set the exact value in set_servos(), but it helps for
logging logging
*/ */
g.channel_throttle.servo_out = g.channel_throttle.control_in; channel_throttle->servo_out = channel_throttle->control_in;
g.channel_steer.servo_out = g.channel_steer.pwm_to_angle(); channel_steer->servo_out = channel_steer->pwm_to_angle();
break; break;
case HOLD: case HOLD:
// hold position - stop motors and center steering // hold position - stop motors and center steering
g.channel_throttle.servo_out = 0; channel_throttle->servo_out = 0;
g.channel_steer.servo_out = 0; channel_steer->servo_out = 0;
break; break;
case INITIALISING: case INITIALISING:
@ -906,7 +915,7 @@ static void update_navigation()
calc_nav_steer(); calc_nav_steer();
calc_bearing_error(); calc_bearing_error();
if (verify_RTL()) { if (verify_RTL()) {
g.channel_throttle.servo_out = g.throttle_min.get(); channel_throttle->servo_out = g.throttle_min.get();
set_mode(HOLD); set_mode(HOLD);
} }
break; break;

6
APMrover2/GCS_Mavlink.pde
View File

@ -275,9 +275,9 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
chan, chan,
millis(), millis(),
0, // port 0 0, // port 0
10000 * g.channel_steer.norm_output(), 10000 * channel_steer->norm_output(),
0, 0,
10000 * g.channel_throttle.norm_output(), 10000 * channel_throttle->norm_output(),
0, 0,
0, 0,
0, 0,
@ -341,7 +341,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
(float)g_gps->ground_speed / 100.0, (float)g_gps->ground_speed / 100.0,
(float)g_gps->ground_speed / 100.0, (float)g_gps->ground_speed / 100.0,
(ahrs.yaw_sensor / 100) % 360, (ahrs.yaw_sensor / 100) % 360,
(uint16_t)(100 * g.channel_throttle.norm_output()), (uint16_t)(100 * channel_throttle->norm_output()),
current_loc.alt / 100.0, current_loc.alt / 100.0,
0); 0);
} }

10
APMrover2/Log.pde
View File

@ -264,10 +264,10 @@ static void Log_Write_Control_Tuning()
Vector3f accel = ins.get_accel(); Vector3f accel = ins.get_accel();
struct log_Control_Tuning pkt = { struct log_Control_Tuning pkt = {
LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG), LOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
steer_out : (int16_t)g.channel_steer.servo_out, steer_out : (int16_t)channel_steer->servo_out,
roll : (int16_t)ahrs.roll_sensor, roll : (int16_t)ahrs.roll_sensor,
pitch : (int16_t)ahrs.pitch_sensor, pitch : (int16_t)ahrs.pitch_sensor,
throttle_out : (int16_t)g.channel_throttle.servo_out, throttle_out : (int16_t)channel_throttle->servo_out,
accel_y : accel.y accel_y : accel.y
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
@ -294,7 +294,7 @@ static void Log_Write_Nav_Tuning()
target_bearing_cd : (uint16_t)target_bearing, target_bearing_cd : (uint16_t)target_bearing,
nav_bearing_cd : (uint16_t)nav_bearing, nav_bearing_cd : (uint16_t)nav_bearing,
nav_gain_scalar : (int16_t)(nav_gain_scaler*1000), nav_gain_scalar : (int16_t)(nav_gain_scaler*1000),
throttle : (int8_t)(100 * g.channel_throttle.norm_output()) throttle : (int8_t)(100 * channel_throttle->norm_output())
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -366,7 +366,7 @@ static void Log_Write_Sonar()
turn_angle : (int8_t)obstacle.turn_angle, turn_angle : (int8_t)obstacle.turn_angle,
turn_time : turn_time, turn_time : turn_time,
ground_speed : (uint16_t)(ground_speed*100), ground_speed : (uint16_t)(ground_speed*100),
throttle : (int8_t)(100 * g.channel_throttle.norm_output()) throttle : (int8_t)(100 * channel_throttle->norm_output())
}; };
DataFlash.WriteBlock(&pkt, sizeof(pkt)); DataFlash.WriteBlock(&pkt, sizeof(pkt));
} }
@ -385,7 +385,7 @@ static void Log_Write_Current()
{ {
struct log_Current pkt = { struct log_Current pkt = {
LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG),
throttle_in : g.channel_throttle.control_in, throttle_in : channel_throttle->control_in,
battery_voltage : (int16_t)(battery_voltage1 * 100.0), battery_voltage : (int16_t)(battery_voltage1 * 100.0),
current_amps : (int16_t)(current_amps1 * 100.0), current_amps : (int16_t)(current_amps1 * 100.0),
board_voltage : board_voltage(), board_voltage : board_voltage(),

13
APMrover2/Parameters.h
View File

@ -76,9 +76,9 @@ public:
// //
// 160: Radio settings // 160: Radio settings
// //
k_param_channel_steer = 160, k_param_rc_1 = 160,
k_param_rc_2, k_param_rc_2,
k_param_channel_throttle, k_param_rc_3,
k_param_rc_4, k_param_rc_4,
k_param_rc_5, k_param_rc_5,
k_param_rc_6, k_param_rc_6,
@ -139,6 +139,7 @@ public:
k_param_ahrs, k_param_ahrs,
k_param_ins, k_param_ins,
k_param_compass, k_param_compass,
k_param_rcmap,
// 254,255: reserved // 254,255: reserved
}; };
@ -187,9 +188,9 @@ public:
AP_Float auto_kickstart; AP_Float auto_kickstart;
// RC channels // RC channels
RC_Channel channel_steer; RC_Channel rc_1;
RC_Channel_aux rc_2; RC_Channel_aux rc_2;
RC_Channel channel_throttle; RC_Channel rc_3;
RC_Channel_aux rc_4; RC_Channel_aux rc_4;
RC_Channel_aux rc_5; RC_Channel_aux rc_5;
RC_Channel_aux rc_6; RC_Channel_aux rc_6;
@ -243,9 +244,9 @@ public:
Parameters() : Parameters() :
// RC channels // RC channels
channel_steer(CH_1), rc_1(CH_1),
rc_2(CH_2), rc_2(CH_2),
channel_throttle(CH_3), rc_3(CH_3),
rc_4(CH_4), rc_4(CH_4),
rc_5(CH_5), rc_5(CH_5),
rc_6(CH_6), rc_6(CH_6),

8
APMrover2/Parameters.pde
View File

@ -197,7 +197,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: RC1_ // @Group: RC1_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp // @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(channel_steer, "RC1_", RC_Channel), GGROUP(rc_1, "RC1_", RC_Channel),
// @Group: RC2_ // @Group: RC2_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp // @Path: ../libraries/RC_Channel/RC_Channel.cpp
@ -205,7 +205,7 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Group: RC3_ // @Group: RC3_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp // @Path: ../libraries/RC_Channel/RC_Channel.cpp
GGROUP(channel_throttle, "RC3_", RC_Channel), GGROUP(rc_3, "RC3_", RC_Channel),
// @Group: RC4_ // @Group: RC4_
// @Path: ../libraries/RC_Channel/RC_Channel.cpp // @Path: ../libraries/RC_Channel/RC_Channel.cpp
@ -416,6 +416,10 @@ const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_Compass/Compass.cpp // @Path: ../libraries/AP_Compass/Compass.cpp
GOBJECT(compass, "COMPASS_", Compass), GOBJECT(compass, "COMPASS_", Compass),
// @Group: RCMAP_
// @Path: ../libraries/AP_RCMapper/AP_RCMapper.cpp
GOBJECT(rcmap, "RCMAP_", RCMapper),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK),

44
APMrover2/Steering.pde
View File

@ -8,12 +8,12 @@ static void throttle_slew_limit(int16_t last_throttle)
// if slew limit rate is set to zero then do not slew limit // if slew limit rate is set to zero then do not slew limit
if (g.throttle_slewrate) { if (g.throttle_slewrate) {
// limit throttle change by the given percentage per second // limit throttle change by the given percentage per second
float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(g.channel_throttle.radio_max - g.channel_throttle.radio_min); float temp = g.throttle_slewrate * G_Dt * 0.01f * fabsf(channel_throttle->radio_max - channel_throttle->radio_min);
// allow a minimum change of 1 PWM per cycle // allow a minimum change of 1 PWM per cycle
if (temp < 1) { if (temp < 1) {
temp = 1; temp = 1;
} }
g.channel_throttle.radio_out = constrain_int16(g.channel_throttle.radio_out, last_throttle - temp, last_throttle + temp); channel_throttle->radio_out = constrain_int16(channel_throttle->radio_out, last_throttle - temp, last_throttle + temp);
} }
} }
@ -71,13 +71,13 @@ static bool auto_check_trigger(void)
static void calc_throttle(float target_speed) static void calc_throttle(float target_speed)
{ {
if (!auto_check_trigger()) { if (!auto_check_trigger()) {
g.channel_throttle.servo_out = g.throttle_min.get(); channel_throttle->servo_out = g.throttle_min.get();
return; return;
} }
if (target_speed <= 0) { if (target_speed <= 0) {
// cope with zero requested speed // cope with zero requested speed
g.channel_throttle.servo_out = g.throttle_min.get(); channel_throttle->servo_out = g.throttle_min.get();
return; return;
} }
@ -110,7 +110,7 @@ static void calc_throttle(float target_speed)
// much faster response in turns // much faster response in turns
throttle *= reduction; throttle *= reduction;
g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get()); channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
} }
/***************************************** /*****************************************
@ -137,7 +137,7 @@ static void calc_nav_steer()
// avoid obstacles, if any // avoid obstacles, if any
nav_steer_cd += obstacle.turn_angle*100; nav_steer_cd += obstacle.turn_angle*100;
g.channel_steer.servo_out = nav_steer_cd; channel_steer->servo_out = nav_steer_cd;
} }
/***************************************** /*****************************************
@ -145,30 +145,30 @@ static void calc_nav_steer()
*****************************************/ *****************************************/
static void set_servos(void) static void set_servos(void)
{ {
int16_t last_throttle = g.channel_throttle.radio_out; int16_t last_throttle = channel_throttle->radio_out;
if ((control_mode == MANUAL || control_mode == LEARNING) && if ((control_mode == MANUAL || control_mode == LEARNING) &&
(g.skid_steer_out == g.skid_steer_in)) { (g.skid_steer_out == g.skid_steer_in)) {
// do a direct pass through of radio values // do a direct pass through of radio values
g.channel_steer.radio_out = hal.rcin->read(CH_STEER); channel_steer->radio_out = channel_steer->read();
g.channel_throttle.radio_out = hal.rcin->read(CH_THROTTLE); channel_throttle->radio_out = channel_throttle->read();
if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) { if (failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
// suppress throttle if in failsafe and manual // suppress throttle if in failsafe and manual
g.channel_throttle.radio_out = g.channel_throttle.radio_trim; channel_throttle->radio_out = channel_throttle->radio_trim;
} }
} else { } else {
g.channel_steer.calc_pwm(); channel_steer->calc_pwm();
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out,
g.throttle_min.get(), g.throttle_min.get(),
g.throttle_max.get()); g.throttle_max.get());
if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) { if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
// suppress throttle if in failsafe // suppress throttle if in failsafe
g.channel_throttle.servo_out = 0; channel_throttle->servo_out = 0;
} }
// convert 0 to 100% into PWM // convert 0 to 100% into PWM
g.channel_throttle.calc_pwm(); channel_throttle->calc_pwm();
// limit throttle movement speed // limit throttle movement speed
throttle_slew_limit(last_throttle); throttle_slew_limit(last_throttle);
@ -182,14 +182,14 @@ static void set_servos(void)
motor1 = throttle + 0.5*steering motor1 = throttle + 0.5*steering
motor2 = throttle - 0.5*steering motor2 = throttle - 0.5*steering
*/ */
float steering_scaled = g.channel_steer.norm_output(); float steering_scaled = channel_steer->norm_output();
float throttle_scaled = g.channel_throttle.norm_output(); float throttle_scaled = channel_throttle->norm_output();
float motor1 = throttle_scaled + 0.5*steering_scaled; float motor1 = throttle_scaled + 0.5*steering_scaled;
float motor2 = throttle_scaled - 0.5*steering_scaled; float motor2 = throttle_scaled - 0.5*steering_scaled;
g.channel_steer.servo_out = 4500*motor1; channel_steer->servo_out = 4500*motor1;
g.channel_throttle.servo_out = 100*motor2; channel_throttle->servo_out = 100*motor2;
g.channel_steer.calc_pwm(); channel_steer->calc_pwm();
g.channel_throttle.calc_pwm(); channel_throttle->calc_pwm();
} }
} }
@ -197,8 +197,8 @@ static void set_servos(void)
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
// send values to the PWM timers for output // send values to the PWM timers for output
// ---------------------------------------- // ----------------------------------------
hal.rcout->write(CH_1, g.channel_steer.radio_out); // send to Servos channel_steer->output();
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos channel_throttle->output();
// Route configurable aux. functions to their respective servos // Route configurable aux. functions to their respective servos
g.rc_2.output_ch(CH_2); g.rc_2.output_ch(CH_2);

2
APMrover2/control_modes.pde
View File

@ -73,7 +73,7 @@ static void read_trim_switch()
g.command_total = 0; g.command_total = 0;
g.command_index =0; g.command_index =0;
nav_command_index = 0; nav_command_index = 0;
if (g.channel_steer.control_in > 3000) { if (channel_steer->control_in > 3000) {
// if roll is full right store the current location as home // if roll is full right store the current location as home
init_home(); init_home();
} }

3
APMrover2/defines.h
View File

@ -42,9 +42,6 @@ enum ch7_option {
#define GPS_PROTOCOL_MTK19 6 #define GPS_PROTOCOL_MTK19 6
#define GPS_PROTOCOL_AUTO 7 #define GPS_PROTOCOL_AUTO 7
#define CH_STEER CH_1
#define CH_THROTTLE CH_3
// HIL enumerations // HIL enumerations
#define HIL_MODE_DISABLED 0 #define HIL_MODE_DISABLED 0
#define HIL_MODE_ATTITUDE 1 #define HIL_MODE_ATTITUDE 1

2
APMrover2/failsafe.pde
View File

@ -36,7 +36,7 @@ void failsafe_check(uint32_t tnow)
} }
if (in_failsafe && tnow - last_timestamp > 20000 && if (in_failsafe && tnow - last_timestamp > 20000 &&
hal.rcin->read(CH_3) >= (uint16_t)g.fs_throttle_value) { channel_throttle->read() >= (uint16_t)g.fs_throttle_value) {
// pass RC inputs to outputs every 20ms // pass RC inputs to outputs every 20ms
last_timestamp = tnow; last_timestamp = tnow;
hal.rcin->clear_overrides(); hal.rcin->clear_overrides();

99
APMrover2/radio.pde
View File

@ -1,14 +1,23 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
allow for runtime change of control channel ordering
*/
static void set_control_channels(void)
{
channel_steer = RC_Channel::rc_channel(rcmap.roll()-1);
channel_throttle = RC_Channel::rc_channel(rcmap.throttle()-1);
}
static void init_rc_in() static void init_rc_in()
{ {
// set rc channel ranges // set rc channel ranges
g.channel_steer.set_angle(SERVO_MAX); channel_steer->set_angle(SERVO_MAX);
g.channel_throttle.set_angle(100); channel_throttle->set_angle(100);
// set rc dead zones // set rc dead zones
g.channel_steer.set_dead_zone(60); channel_steer->set_dead_zone(60);
g.channel_throttle.set_dead_zone(6); channel_throttle->set_dead_zone(6);
//set auxiliary ranges //set auxiliary ranges
update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8); update_aux_servo_function(&g.rc_2, &g.rc_4, &g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
@ -16,58 +25,24 @@ static void init_rc_in()
static void init_rc_out() static void init_rc_out()
{ {
hal.rcout->enable_ch(CH_1); for (uint8_t i=0; i<8; i++) {
hal.rcout->enable_ch(CH_2); RC_Channel::rc_channel(i)->enable_out();
hal.rcout->enable_ch(CH_3); RC_Channel::rc_channel(i)->output_trim();
hal.rcout->enable_ch(CH_4); }
hal.rcout->enable_ch(CH_5);
hal.rcout->enable_ch(CH_6);
hal.rcout->enable_ch(CH_7);
hal.rcout->enable_ch(CH_8);
#if HIL_MODE != HIL_MODE_ATTITUDE
hal.rcout->write(CH_1, g.channel_steer.radio_trim); // Initialization of servo outputs
hal.rcout->write(CH_3, g.channel_throttle.radio_trim);
hal.rcout->write(CH_2, g.rc_2.radio_trim);
hal.rcout->write(CH_4, g.rc_4.radio_trim);
hal.rcout->write(CH_5, g.rc_5.radio_trim);
hal.rcout->write(CH_6, g.rc_6.radio_trim);
hal.rcout->write(CH_7, g.rc_7.radio_trim);
hal.rcout->write(CH_8, g.rc_8.radio_trim);
#else
hal.rcout->write(CH_1, 1500); // Initialization of servo outputs
hal.rcout->write(CH_2, 1500);
hal.rcout->write(CH_3, 1000);
hal.rcout->write(CH_4, 1500);
hal.rcout->write(CH_5, 1500);
hal.rcout->write(CH_6, 1500);
hal.rcout->write(CH_7, 1500);
hal.rcout->write(CH_8, 2000);
#endif
} }
static void read_radio() static void read_radio()
{ {
g.channel_steer.set_pwm(hal.rcin->read(CH_STEER)); for (uint8_t i=0; i<8; i++) {
RC_Channel::rc_channel(i)->set_pwm(RC_Channel::rc_channel(i)->read());
}
g.channel_throttle.set_pwm(hal.rcin->read(CH_3)); control_failsafe(channel_throttle->radio_in);
g.rc_2.set_pwm(hal.rcin->read(CH_2)); channel_throttle->servo_out = channel_throttle->control_in;
g.rc_4.set_pwm(hal.rcin->read(CH_4));
g.rc_5.set_pwm(hal.rcin->read(CH_5));
g.rc_6.set_pwm(hal.rcin->read(CH_6));
g.rc_7.set_pwm(hal.rcin->read(CH_7));
g.rc_8.set_pwm(hal.rcin->read(CH_8));
control_failsafe(g.channel_throttle.radio_in); if (channel_throttle->servo_out > 50) {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((channel_throttle->norm_input()-0.5) / 0.5);
g.channel_throttle.servo_out = g.channel_throttle.control_in;
if (g.channel_throttle.servo_out > 50) {
throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
} else { } else {
throttle_nudge = 0; throttle_nudge = 0;
} }
@ -82,24 +57,24 @@ static void read_radio()
motor2 = throttle - 0.5*steering motor2 = throttle - 0.5*steering
*/ */
float motor1 = g.channel_steer.norm_input(); float motor1 = channel_steer->norm_input();
float motor2 = g.channel_throttle.norm_input(); float motor2 = channel_throttle->norm_input();
float steering_scaled = motor1 - motor2; float steering_scaled = motor1 - motor2;
float throttle_scaled = 0.5f*(motor1 + motor2); float throttle_scaled = 0.5f*(motor1 + motor2);
int16_t steer = g.channel_steer.radio_trim; int16_t steer = channel_steer->radio_trim;
int16_t thr = g.channel_throttle.radio_trim; int16_t thr = channel_throttle->radio_trim;
if (steering_scaled > 0.0f) { if (steering_scaled > 0.0f) {
steer += steering_scaled*(g.channel_steer.radio_max-g.channel_steer.radio_trim); steer += steering_scaled*(channel_steer->radio_max-channel_steer->radio_trim);
} else { } else {
steer += steering_scaled*(g.channel_steer.radio_trim-g.channel_steer.radio_min); steer += steering_scaled*(channel_steer->radio_trim-channel_steer->radio_min);
} }
if (throttle_scaled > 0.0f) { if (throttle_scaled > 0.0f) {
thr += throttle_scaled*(g.channel_throttle.radio_max-g.channel_throttle.radio_trim); thr += throttle_scaled*(channel_throttle->radio_max-channel_throttle->radio_trim);
} else { } else {
thr += throttle_scaled*(g.channel_throttle.radio_trim-g.channel_throttle.radio_min); thr += throttle_scaled*(channel_throttle->radio_trim-channel_throttle->radio_min);
} }
g.channel_steer.set_pwm(steer); channel_steer->set_pwm(steer);
g.channel_throttle.set_pwm(thr); channel_throttle->set_pwm(thr);
} }
} }
@ -123,10 +98,10 @@ static void trim_control_surfaces()
read_radio(); read_radio();
// Store control surface trim values // Store control surface trim values
// --------------------------------- // ---------------------------------
if (g.channel_steer.radio_in > 1400) { if (channel_steer->radio_in > 1400) {
g.channel_steer.radio_trim = g.channel_steer.radio_in; channel_steer->radio_trim = channel_steer->radio_in;
// save to eeprom // save to eeprom
g.channel_steer.save_eeprom(); channel_steer->save_eeprom();
} }
} }

28
APMrover2/setup.pde
View File

@ -225,7 +225,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
} }
if(g.channel_steer.radio_in < 500){ if(channel_steer->radio_in < 500){
while(1){ while(1){
cliSerial->printf_P(PSTR("\nNo radio; Check connectors.")); cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
delay(1000); delay(1000);
@ -233,8 +233,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
} }
} }
g.channel_steer.radio_min = g.channel_steer.radio_in; channel_steer->radio_min = channel_steer->radio_in;
g.channel_throttle.radio_min = g.channel_throttle.radio_in; channel_throttle->radio_min = channel_throttle->radio_in;
g.rc_2.radio_min = g.rc_2.radio_in; g.rc_2.radio_min = g.rc_2.radio_in;
g.rc_4.radio_min = g.rc_4.radio_in; g.rc_4.radio_min = g.rc_4.radio_in;
g.rc_5.radio_min = g.rc_5.radio_in; g.rc_5.radio_min = g.rc_5.radio_in;
@ -242,8 +242,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_7.radio_min = g.rc_7.radio_in; g.rc_7.radio_min = g.rc_7.radio_in;
g.rc_8.radio_min = g.rc_8.radio_in; g.rc_8.radio_min = g.rc_8.radio_in;
g.channel_steer.radio_max = g.channel_steer.radio_in; channel_steer->radio_max = channel_steer->radio_in;
g.channel_throttle.radio_max = g.channel_throttle.radio_in; channel_throttle->radio_max = channel_throttle->radio_in;
g.rc_2.radio_max = g.rc_2.radio_in; g.rc_2.radio_max = g.rc_2.radio_in;
g.rc_4.radio_max = g.rc_4.radio_in; g.rc_4.radio_max = g.rc_4.radio_in;
g.rc_5.radio_max = g.rc_5.radio_in; g.rc_5.radio_max = g.rc_5.radio_in;
@ -251,7 +251,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
g.rc_7.radio_max = g.rc_7.radio_in; g.rc_7.radio_max = g.rc_7.radio_in;
g.rc_8.radio_max = g.rc_8.radio_in; g.rc_8.radio_max = g.rc_8.radio_in;
g.channel_steer.radio_trim = g.channel_steer.radio_in; channel_steer->radio_trim = channel_steer->radio_in;
g.rc_2.radio_trim = 1500; g.rc_2.radio_trim = 1500;
g.rc_4.radio_trim = 1500; g.rc_4.radio_trim = 1500;
g.rc_5.radio_trim = 1500; g.rc_5.radio_trim = 1500;
@ -267,8 +267,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
// ---------------------------------------------------------- // ----------------------------------------------------------
read_radio(); read_radio();
g.channel_steer.update_min_max(); channel_steer->update_min_max();
g.channel_throttle.update_min_max(); channel_throttle->update_min_max();
g.rc_2.update_min_max(); g.rc_2.update_min_max();
g.rc_4.update_min_max(); g.rc_4.update_min_max();
g.rc_5.update_min_max(); g.rc_5.update_min_max();
@ -280,8 +280,8 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
while (cliSerial->available() > 0) { while (cliSerial->available() > 0) {
cliSerial->read(); cliSerial->read();
} }
g.channel_steer.save_eeprom(); channel_steer->save_eeprom();
g.channel_throttle.save_eeprom(); channel_throttle->save_eeprom();
g.rc_2.save_eeprom(); g.rc_2.save_eeprom();
g.rc_4.save_eeprom(); g.rc_4.save_eeprom();
g.rc_5.save_eeprom(); g.rc_5.save_eeprom();
@ -618,9 +618,9 @@ print_PID(PID * pid)
static void static void
print_radio_values() print_radio_values()
{ {
cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_steer.radio_min, (int)g.channel_steer.radio_trim, (int)g.channel_steer.radio_max); cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)channel_steer->radio_min, (int)channel_steer->radio_trim, (int)channel_steer->radio_max);
cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_trim, (int)g.rc_2.radio_max); cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.rc_2.radio_min, (int)g.rc_2.radio_trim, (int)g.rc_2.radio_max);
cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)g.channel_throttle.radio_min, (int)g.channel_throttle.radio_trim, (int)g.channel_throttle.radio_max); cliSerial->printf_P(PSTR("CH3: %d | %d | %d\n"), (int)channel_throttle->radio_min, (int)channel_throttle->radio_trim, (int)channel_throttle->radio_max);
cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_trim, (int)g.rc_4.radio_max); cliSerial->printf_P(PSTR("CH4: %d | %d | %d\n"), (int)g.rc_4.radio_min, (int)g.rc_4.radio_trim, (int)g.rc_4.radio_max);
cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max); cliSerial->printf_P(PSTR("CH5: %d | %d | %d\n"), (int)g.rc_5.radio_min, (int)g.rc_5.radio_trim, (int)g.rc_5.radio_max);
cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max); cliSerial->printf_P(PSTR("CH6: %d | %d | %d\n"), (int)g.rc_6.radio_min, (int)g.rc_6.radio_trim, (int)g.rc_6.radio_max);
@ -667,10 +667,10 @@ radio_input_switch(void)
static int8_t bouncer = 0; static int8_t bouncer = 0;
if (int16_t(g.channel_steer.radio_in - g.channel_steer.radio_trim) > 100) { if (int16_t(channel_steer->radio_in - channel_steer->radio_trim) > 100) {
bouncer = 10; bouncer = 10;
} }
if (int16_t(g.channel_steer.radio_in - g.channel_steer.radio_trim) < -100) { if (int16_t(channel_steer->radio_in - channel_steer->radio_trim) < -100) {
bouncer = -10; bouncer = -10;
} }
if (bouncer >0) { if (bouncer >0) {

2
APMrover2/system.pde
View File

@ -122,6 +122,8 @@ static void init_ardupilot()
load_parameters(); load_parameters();
set_control_channels();
// after parameter load setup correct baud rate on uartA // after parameter load setup correct baud rate on uartA
hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD)); hal.uartA->begin(map_baudrate(g.serial0_baud, SERIAL0_BAUD));

22
APMrover2/test.pde
View File

@ -91,9 +91,9 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
read_radio(); read_radio();
cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"), cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
g.channel_steer.radio_in, channel_steer->radio_in,
g.rc_2.radio_in, g.rc_2.radio_in,
g.channel_throttle.radio_in, channel_throttle->radio_in,
g.rc_4.radio_in, g.rc_4.radio_in,
g.rc_5.radio_in, g.rc_5.radio_in,
g.rc_6.radio_in, g.rc_6.radio_in,
@ -147,8 +147,8 @@ test_radio(uint8_t argc, const Menu::arg *argv)
delay(20); delay(20);
read_radio(); read_radio();
g.channel_steer.calc_pwm(); channel_steer->calc_pwm();
g.channel_throttle.calc_pwm(); channel_throttle->calc_pwm();
// write out the servo PWM values // write out the servo PWM values
// ------------------------------ // ------------------------------
@ -157,9 +157,9 @@ test_radio(uint8_t argc, const Menu::arg *argv)
tuning_value = constrain_float(((float)(g.rc_7.radio_in - g.rc_7.radio_min) / (float)(g.rc_7.radio_max - g.rc_7.radio_min)),0,1); tuning_value = constrain_float(((float)(g.rc_7.radio_in - g.rc_7.radio_min) / (float)(g.rc_7.radio_max - g.rc_7.radio_min)),0,1);
cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d Tuning = %2.3f\n"), cliSerial->printf_P(PSTR("IN 1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d Tuning = %2.3f\n"),
g.channel_steer.control_in, channel_steer->control_in,
g.rc_2.control_in, g.rc_2.control_in,
g.channel_throttle.control_in, channel_throttle->control_in,
g.rc_4.control_in, g.rc_4.control_in,
g.rc_5.control_in, g.rc_5.control_in,
g.rc_6.control_in, g.rc_6.control_in,
@ -190,7 +190,7 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
oldSwitchPosition = readSwitch(); oldSwitchPosition = readSwitch();
cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n")); cliSerial->printf_P(PSTR("Unplug battery, throttle in neutral, turn off radio.\n"));
while(g.channel_throttle.control_in > 0){ while(channel_throttle->control_in > 0){
delay(20); delay(20);
read_radio(); read_radio();
} }
@ -199,8 +199,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
delay(20); delay(20);
read_radio(); read_radio();
if(g.channel_throttle.control_in > 0){ if(channel_throttle->control_in > 0){
cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), g.channel_throttle.control_in); cliSerial->printf_P(PSTR("THROTTLE CHANGED %d \n"), channel_throttle->control_in);
fail_test++; fail_test++;
} }
@ -211,8 +211,8 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
fail_test++; fail_test++;
} }
if (g.fs_throttle_enabled && g.channel_throttle.get_failsafe()){ if (g.fs_throttle_enabled && channel_throttle->get_failsafe()){
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in); cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), channel_throttle->radio_in);
print_mode(cliSerial, readSwitch()); print_mode(cliSerial, readSwitch());
cliSerial->println(); cliSerial->println();
fail_test++; fail_test++;