Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: added HIL_SERVOS option

Browse Source 
this allows for real servo output in HIL
This commit is contained in:
Andrew Tridgell 2013-03-30 14:38:43 +11:00
parent 645bd87b63
commit dfb14d760b
11 changed files with 74 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
ArduPlane/Attitude.pde
View File

@ -629,7 +629,12 @@ static void set_servos(void)
throttle_slew_limit(last_throttle);
}
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
#if HIL_MODE != HIL_MODE_DISABLED
if (!g.hil_servos) {
return;
}
#endif
// send values to the PWM timers for output
// ----------------------------------------
hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos
@ -646,7 +651,6 @@ static void set_servos(void)
g.rc_10.output_ch(CH_10);
g.rc_11.output_ch(CH_11);
# endif
#endif
}
static bool demoing_servos;
@ -656,13 +660,11 @@ static void demo_servos(uint8_t i) {
while(i > 0) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Demo Servos!"));
demoing_servos = true;
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
hal.rcout->write(1, 1400);
servo_write(1, 1400);
mavlink_delay(400);
hal.rcout->write(1, 1600);
servo_write(1, 1600);
mavlink_delay(200);
hal.rcout->write(1, 1500);
#endif
servo_write(1, 1500);
demoing_servos = false;
mavlink_delay(400);
i--;

37
ArduPlane/GCS_Mavlink.pde
View File

@ -339,7 +339,24 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
static void NOINLINE send_radio_out(mavlink_channel_t chan)
{
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
#if HIL_MODE != HIL_MODE_DISABLED
if (!g.hil_servos) {
extern RC_Channel* rc_ch[8];
mavlink_msg_servo_output_raw_send(
chan,
micros(),
0, // port
rc_ch[0]->radio_out,
rc_ch[1]->radio_out,
rc_ch[2]->radio_out,
rc_ch[3]->radio_out,
rc_ch[4]->radio_out,
rc_ch[5]->radio_out,
rc_ch[6]->radio_out,
rc_ch[7]->radio_out);
return;
}
#endif
mavlink_msg_servo_output_raw_send(
chan,
micros(),
@ -352,21 +369,6 @@ static void NOINLINE send_radio_out(mavlink_channel_t chan)
hal.rcout->read(5),
hal.rcout->read(6),
hal.rcout->read(7));
#else
extern RC_Channel* rc_ch[8];
mavlink_msg_servo_output_raw_send(
chan,
micros(),
0, // port
rc_ch[0]->radio_out,
rc_ch[1]->radio_out,
rc_ch[2]->radio_out,
rc_ch[3]->radio_out,
rc_ch[4]->radio_out,
rc_ch[5]->radio_out,
rc_ch[6]->radio_out,
rc_ch[7]->radio_out);
#endif
}
static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
@ -1103,8 +1105,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
case MAV_CMD_DO_SET_SERVO:
hal.rcout->enable_ch(packet.param1 - 1);
hal.rcout->write(packet.param1 - 1, packet.param2);
servo_write(packet.param1 - 1, packet.param2);
result = MAV_RESULT_ACCEPTED;
break;

4
ArduPlane/Parameters.h
View File

@ -81,6 +81,7 @@ public:
k_param_takeoff_throttle_min_speed,
k_param_takeoff_throttle_min_accel,
k_param_takeoff_heading_hold,
k_param_hil_servos,
// 110: Telemetry control
//
@ -342,6 +343,9 @@ public:
AP_Int32 min_gndspeed_cm;
AP_Int16 pitch_trim_cd;
AP_Int16 FBWB_min_altitude_cm;
#if HIL_MODE != HIL_MODE_DISABLED
AP_Int8 hil_servos;
#endif
AP_Int8 compass_enabled;
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current

9
ArduPlane/Parameters.pde
View File

@ -624,6 +624,15 @@ const AP_Param::Info var_info[] PROGMEM = {
GSCALAR(inverted_flight_ch, "INVERTEDFLT_CH", 0),
#if HIL_MODE != HIL_MODE_DISABLED
// @Param: HIL_SERVOS
// @DisplayName: HIL Servos enable
// @Description: This controls whether real servo controls are used in HIL mode. If you enable this then the APM will control the real servos in HIL mode. If disabled it will report servo values, but will not output to the real servos. Be careful that your motor and propeller are not connected if you enable this option.
// @Values: 0:Disabled,1:Enabled
// @User: Advanced
GSCALAR(hil_servos, "HIL_SERVOS", 0),
#endif
// barometer ground calibration. The GND_ prefix is chosen for
// compatibility with previous releases of ArduPlane
GOBJECT(barometer, "GND_", AP_Baro),

3
ArduPlane/commands_logic.pde
View File

@ -596,8 +596,7 @@ static void do_set_home()
static void do_set_servo()
{
hal.rcout->enable_ch(next_nonnav_command.p1 - 1);
hal.rcout->write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
servo_write(next_nonnav_command.p1 - 1, next_nonnav_command.alt);
}
static void do_set_relay()

5
ArduPlane/config.h
View File

@ -794,11 +794,6 @@
# define SCALING_SPEED 15.0
#endif
// use this to enable servos in HIL mode
#ifndef HIL_SERVOS
# define HIL_SERVOS DISABLED
#endif
// use this to completely disable the CLI
#ifndef CLI_ENABLED
# define CLI_ENABLED ENABLED

4
ArduPlane/events.pde
View File

@ -109,9 +109,9 @@ static void update_events(void)
case EVENT_TYPE_SERVO:
hal.rcout->enable_ch(event_state.rc_channel);
if (event_state.repeat & 1) {
hal.rcout->write(event_state.rc_channel, event_state.undo_value);
servo_write(event_state.rc_channel, event_state.undo_value);
} else {
hal.rcout->write(event_state.rc_channel, event_state.servo_value);
servo_write(event_state.rc_channel, event_state.servo_value);
}
break;

2
ArduPlane/failsafe.pde
View File

@ -44,7 +44,7 @@ void failsafe_check(uint32_t tnow)
start_ch = 1;
}
for (uint8_t ch=start_ch; ch<4; ch++) {
hal.rcout->write(ch, hal.rcin->read(ch));
servo_write(ch, hal.rcin->read(ch));
}
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_manual, true);
RC_Channel_aux::copy_radio_in_out(RC_Channel_aux::k_aileron_with_input, true);

22
ArduPlane/radio.pde
View File

@ -52,20 +52,20 @@ static void init_rc_out()
enable_aux_servos();
// Initialization of servo outputs
hal.rcout->write(CH_1, g.channel_roll.radio_trim);
hal.rcout->write(CH_2, g.channel_pitch.radio_trim);
hal.rcout->write(CH_3, g.channel_throttle.radio_min);
hal.rcout->write(CH_4, g.channel_rudder.radio_trim);
servo_write(CH_1, g.channel_roll.radio_trim);
servo_write(CH_2, g.channel_pitch.radio_trim);
servo_write(CH_3, g.channel_throttle.radio_min);
servo_write(CH_4, g.channel_rudder.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);
servo_write(CH_5, g.rc_5.radio_trim);
servo_write(CH_6, g.rc_6.radio_trim);
servo_write(CH_7, g.rc_7.radio_trim);
servo_write(CH_8, g.rc_8.radio_trim);
#if CONFIG_HAL_BOARD == HAL_BOARD_APM2
hal.rcout->write(CH_9, g.rc_9.radio_trim);
hal.rcout->write(CH_10, g.rc_10.radio_trim);
hal.rcout->write(CH_11, g.rc_11.radio_trim);
servo_write(CH_9, g.rc_9.radio_trim);
servo_write(CH_10, g.rc_10.radio_trim);
servo_write(CH_11, g.rc_11.radio_trim);
#endif
}

17
ArduPlane/system.pde
View File

@ -641,3 +641,20 @@ static void print_comma(void)
}
/*
write to a servo
*/
static void servo_write(uint8_t ch, uint16_t pwm)
{
#if HIL_MODE != HIL_MODE_DISABLED
if (!g.hil_servos) {
extern RC_Channel *rc_ch[8];
if (ch < 8) {
rc_ch[ch]->radio_out = pwm;
}
return;
}
#endif
hal.rcout->enable_ch(ch);
hal.rcout->write(ch, pwm);
}

2
ArduPlane/test.pde
View File

@ -145,7 +145,7 @@ test_passthru(uint8_t argc, const Menu::arg *argv)
for(int16_t i = 0; i < 8; i++) {
cliSerial->print(hal.rcin->read(i)); // Print channel values
print_comma();
hal.rcout->write(i, hal.rcin->read(i)); // Copy input to Servos
servo_write(i, hal.rcin->read(i)); // Copy input to Servos
}
cliSerial->println();
}