mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-02 22:18:28 -04:00
Rover: major restructuring
this removes a lot of the old arduplane code, and renames a lot of variables. This will need a lot more testing and fixup before being usable
This commit is contained in:
Andrew Tridgell
parent
19a282dad0
commit
f1ff27ed08
@ -3,12 +3,4 @@
|
||||
// This file is just a placeholder for your configuration file. If you wish to change any of the setup parameters from
|
||||
// their default values, place the appropriate #define statements here.
|
||||
|
||||
#define LITE DISABLED // if LITE is ENABLED, you may use an APM1280 or APM2560 CPU only (IMU less) with a GPS MT3329
|
||||
// if LITE is DISABLED, this is for a full APM v1 (Oilpan + GPS MT3329 + Magnetometer HMC5883L) or APM v2
|
||||
|
||||
#define CLI_ENABLED ENABLED
|
||||
#define CLI_SLIDER_ENABLED DISABLED
|
||||
#define AUTO_WP_RADIUS DISABLED
|
||||
|
||||
#define TRACE DISABLED
|
||||
|
||||
|
||||
@ -39,7 +39,7 @@ version 2.1 of the License, or (at your option) any later version.
|
||||
// 2012-04-30: Successfully tested in autonomous nav with a waypoints list recorded in live mode
|
||||
// 2012-04-30: Now a full version for APM v1 or APM v2 with magnetometer
|
||||
// 2012-04-27: Cosmetic changes
|
||||
// 2012-04-26: Only one PID (pidNavRoll) for steering the wheel with nav_roll
|
||||
// 2012-04-26: Only one PID (pidNavRoll) for steering the wheel with nav_steer
|
||||
// 2012-04-26: Added ground_speed and ground_course variables in Update_GPS
|
||||
// 2012-04-26: Set GPS to 10 Hz (updated in the AP_GPS lib)
|
||||
// 2012-04-22: Tested on Traxxas Monster Jam Grinder XL-5 3602
|
||||
@ -178,7 +178,7 @@ DataFlash_Empty DataFlash;
|
||||
static GPS *g_gps;
|
||||
|
||||
// flight modes convenience array
|
||||
static AP_Int8 *flight_modes = &g.flight_mode1;
|
||||
static AP_Int8 *modes = &g.mode1;
|
||||
|
||||
#if HIL_MODE == HIL_MODE_DISABLED
|
||||
|
||||
@ -310,7 +310,7 @@ static bool usb_connected;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// This is the state of the flight control system
|
||||
// There are multiple states defined such as MANUAL, FBW-A, AUTO
|
||||
uint8_t control_mode = INITIALISING;
|
||||
enum mode control_mode = INITIALISING;
|
||||
// Used to maintain the state of the previous control switch position
|
||||
// This is set to -1 when we need to re-read the switch
|
||||
uint8_t oldSwitchPosition;
|
||||
@ -391,6 +391,7 @@ static uint8_t non_nav_command_index;
|
||||
static uint8_t nav_command_ID = NO_COMMAND;
|
||||
static uint8_t non_nav_command_ID = NO_COMMAND;
|
||||
|
||||
// ground speed error in m/s
|
||||
static float groundspeed_error;
|
||||
// 0-(throttle_max - throttle_cruise) : throttle nudge in Auto mode using top 1/2 of throttle stick travel
|
||||
static int16_t throttle_nudge = 0;
|
||||
@ -408,9 +409,8 @@ static bool obstacle = false;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Ground speed
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The amount current ground speed is below min ground speed. Centimeters per second
|
||||
static int32_t groundspeed_undershoot = 0;
|
||||
static int32_t ground_speed = 0;
|
||||
// The amount current ground speed is below min ground speed. meters per second
|
||||
static float ground_speed = 0;
|
||||
static int16_t throttle_last = 0, throttle = 500;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
@ -430,7 +430,7 @@ static float crosstrack_error;
|
||||
// Used to track the CH7 toggle state.
|
||||
// When CH7 goes LOW PWM from HIGH PWM, this value will have been set true
|
||||
// This allows advanced functionality to know when to execute
|
||||
static bool trim_flag;
|
||||
static bool ch7_flag;
|
||||
// This register tracks the current Mission Command index when writing
|
||||
// a mission using CH7 in flight
|
||||
static int8_t CH7_wp_index;
|
||||
@ -453,21 +453,18 @@ static float current_total1;
|
||||
|
||||
// JLN Update
|
||||
uint32_t timesw = 0;
|
||||
static bool speed_boost = false;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation control variables
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// The instantaneous desired bank angle. Hundredths of a degree
|
||||
static int32_t nav_roll;
|
||||
// Calculated radius for the wp turn based on ground speed and max turn angle
|
||||
static int32_t wp_radius;
|
||||
static int32_t nav_steer;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Waypoint distances
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Distance between plane and next waypoint. Meters
|
||||
static int32_t wp_distance;
|
||||
static float wp_distance;
|
||||
// Distance between previous and next waypoint. Meters
|
||||
static int32_t wp_totalDistance;
|
||||
|
||||
@ -671,10 +668,6 @@ static void fast_loop()
|
||||
// some space available
|
||||
gcs_send_message(MSG_RETRY_DEFERRED);
|
||||
|
||||
// check for loss of control signal failsafe condition
|
||||
// ------------------------------------
|
||||
check_short_failsafe();
|
||||
|
||||
#if HIL_MODE == HIL_MODE_SENSORS
|
||||
// update hil before dcm update
|
||||
gcs_update();
|
||||
@ -718,12 +711,12 @@ static void fast_loop()
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
update_current_flight_mode();
|
||||
update_current_mode();
|
||||
|
||||
// apply desired roll, pitch and yaw to the plane
|
||||
// ----------------------------------------------
|
||||
if (control_mode > LEARNING)
|
||||
learning();
|
||||
// apply desired steering if in an auto mode
|
||||
if (control_mode >= AUTO) {
|
||||
g.channel_steer.servo_out = nav_steer;
|
||||
}
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
@ -751,7 +744,6 @@ static void medium_loop()
|
||||
case 0:
|
||||
medium_loopCounter++;
|
||||
update_GPS();
|
||||
calc_gndspeed_undershoot();
|
||||
|
||||
//#if LITE == DISABLED
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
@ -878,13 +870,6 @@ static void slow_loop()
|
||||
check_usb_mux();
|
||||
#endif
|
||||
|
||||
#if TRACE == ENABLED
|
||||
// cliSerial->printf_P(PSTR("NAV->gnd_crs=%3.0f, nav_brg=%3.0f, tgt_brg=%3.0f, brg_err=%3.0f, nav_rll=%3.1f rsvo=%3.1f\n"),
|
||||
// ahrs.yaw_sensor*0.01, (float)nav_bearing/100, (float)target_bearing/100, (float)bearing_error/100, (float)nav_roll/100, (float)g.channel_roll.servo_out/100);
|
||||
// cliSerial->printf_P(PSTR("WPL->g.command_total=%d, g.command_index=%d, nav_command_index=%d\n"),
|
||||
// g.command_total, g.command_index, nav_command_index);
|
||||
cliSerial->printf_P(PSTR("NAV->gnd_crs=%3.0f, sonar_dist = %d obstacle = %d\n"), ahrs.yaw_sensor*0.01, (int)sonar_dist, obstacle);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -929,25 +914,27 @@ static void update_GPS(void)
|
||||
ground_start_count = 0;
|
||||
}
|
||||
}
|
||||
ground_speed = g_gps->ground_speed;
|
||||
ground_speed = g_gps->ground_speed * 0.01;
|
||||
}
|
||||
}
|
||||
|
||||
static void update_current_flight_mode(void)
|
||||
static void update_current_mode(void)
|
||||
{
|
||||
switch(control_mode){
|
||||
switch (control_mode){
|
||||
case AUTO:
|
||||
case RTL:
|
||||
calc_nav_roll();
|
||||
case GUIDED:
|
||||
calc_nav_steer();
|
||||
calc_throttle();
|
||||
break;
|
||||
|
||||
case LEARNING:
|
||||
case MANUAL:
|
||||
nav_roll = 0;
|
||||
g.channel_roll.servo_out = g.channel_roll.pwm_to_angle();
|
||||
g.channel_pitch.servo_out = g.channel_pitch.pwm_to_angle();
|
||||
g.channel_rudder.servo_out = g.channel_roll.pwm_to_angle();
|
||||
nav_steer = 0;
|
||||
g.channel_steer.servo_out = g.channel_steer.pwm_to_angle();
|
||||
break;
|
||||
|
||||
case INITIALISING:
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -955,6 +942,11 @@ static void update_current_flight_mode(void)
|
||||
static void update_navigation()
|
||||
{
|
||||
switch (control_mode) {
|
||||
case MANUAL:
|
||||
case LEARNING:
|
||||
case INITIALISING:
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
verify_commands();
|
||||
break;
|
||||
@ -962,9 +954,9 @@ static void update_navigation()
|
||||
case RTL:
|
||||
case GUIDED:
|
||||
// no loitering around the wp with the rover, goes direct to the wp position
|
||||
calc_nav_roll();
|
||||
calc_nav_steer();
|
||||
calc_bearing_error();
|
||||
if(verify_RTL()) {
|
||||
if (verify_RTL()) {
|
||||
g.channel_throttle.servo_out = g.throttle_min.get();
|
||||
set_mode(MANUAL);
|
||||
}
|
||||
|
||||
@ -1,20 +1,5 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
//****************************************************************
|
||||
// Function that controls aileron/rudder, elevator, rudder (if 4 channel control) and throttle to produce desired attitude and airspeed.
|
||||
//****************************************************************
|
||||
|
||||
static void learning()
|
||||
{
|
||||
// Calculate desired servo output for the turn // Wheels Direction
|
||||
// ---------------------------------------------
|
||||
|
||||
g.channel_roll.servo_out = nav_roll;
|
||||
g.channel_roll.servo_out = g.channel_roll.servo_out * g.turn_gain;
|
||||
g.channel_rudder.servo_out = g.channel_roll.servo_out;
|
||||
}
|
||||
|
||||
|
||||
/*****************************************
|
||||
* Throttle slew limit
|
||||
*****************************************/
|
||||
@ -34,28 +19,22 @@ static void throttle_slew_limit(int16_t last_throttle)
|
||||
|
||||
static void calc_throttle()
|
||||
{
|
||||
int rov_speed;
|
||||
int throttle_target = g.throttle_cruise + throttle_nudge;
|
||||
|
||||
rov_speed = g.airspeed_cruise;
|
||||
groundspeed_error = g.speed_cruise - ground_speed;
|
||||
|
||||
if (speed_boost)
|
||||
rov_speed *= g.booster;
|
||||
|
||||
groundspeed_error = rov_speed - (float)ground_speed;
|
||||
|
||||
throttle = throttle_target + g.pidTeThrottle.get_pid(groundspeed_error);
|
||||
g.channel_throttle.servo_out = constrain_int16(throttle, 0, g.throttle_max.get());
|
||||
throttle = throttle_target + g.pidSpeedThrottle.get_pid(groundspeed_error);
|
||||
g.channel_throttle.servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
|
||||
}
|
||||
|
||||
/*****************************************
|
||||
* Calculate desired turn angles (in medium freq loop)
|
||||
*****************************************/
|
||||
|
||||
static void calc_nav_roll()
|
||||
static void calc_nav_steer()
|
||||
{
|
||||
// Adjust gain based on ground speed
|
||||
nav_gain_scaler = (float)ground_speed / (g.airspeed_cruise * 100.0);
|
||||
nav_gain_scaler = (float)ground_speed / g.speed_cruise;
|
||||
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
|
||||
|
||||
// Calculate the required turn of the wheels rover
|
||||
@ -64,21 +43,19 @@ static void calc_nav_roll()
|
||||
// negative error = left turn
|
||||
// positive error = right turn
|
||||
|
||||
nav_roll = g.pidNavRoll.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100
|
||||
nav_steer = g.pidNavSteer.get_pid(bearing_error, nav_gain_scaler); //returns desired bank angle in degrees*100
|
||||
|
||||
if(obstacle) { // obstacle avoidance
|
||||
nav_roll += 9000; // if obstacle in front turn 90° right
|
||||
speed_boost = false;
|
||||
if (obstacle) { // obstacle avoidance
|
||||
nav_steer += 9000; // if obstacle in front turn 90° right
|
||||
}
|
||||
nav_roll = constrain_int16(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
|
||||
}
|
||||
|
||||
// Zeros out navigation Integrators if we are changing mode, have passed a waypoint, etc.
|
||||
// Keeps outdated data out of our calculations
|
||||
static void reset_I(void)
|
||||
{
|
||||
g.pidNavRoll.reset_I();
|
||||
g.pidTeThrottle.reset_I();
|
||||
g.pidNavSteer.reset_I();
|
||||
g.pidSpeedThrottle.reset_I();
|
||||
}
|
||||
|
||||
/*****************************************
|
||||
@ -88,24 +65,20 @@ static void set_servos(void)
|
||||
{
|
||||
int16_t last_throttle = g.channel_throttle.radio_out;
|
||||
|
||||
if((control_mode == MANUAL) || (control_mode == LEARNING)){
|
||||
if ((control_mode == MANUAL) || (control_mode == LEARNING)) {
|
||||
// do a direct pass through of radio values
|
||||
g.channel_roll.radio_out = g.channel_roll.radio_in;
|
||||
g.channel_steer.radio_out = g.channel_steer.radio_in;
|
||||
|
||||
if(obstacle) // obstacle in front, turn right in Stabilize mode
|
||||
g.channel_roll.radio_out -= 500;
|
||||
|
||||
g.channel_pitch.radio_out = g.channel_pitch.radio_in;
|
||||
if (obstacle) // obstacle in front, turn right in Stabilize mode
|
||||
g.channel_steer.radio_out -= 500;
|
||||
|
||||
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
|
||||
g.channel_rudder.radio_out = g.channel_roll.radio_in;
|
||||
} else {
|
||||
g.channel_roll.calc_pwm();
|
||||
g.channel_pitch.calc_pwm();
|
||||
g.channel_rudder.calc_pwm();
|
||||
|
||||
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
|
||||
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
|
||||
g.channel_steer.calc_pwm();
|
||||
g.channel_throttle.radio_out = g.channel_throttle.radio_in;
|
||||
g.channel_throttle.servo_out = constrain_int16(g.channel_throttle.servo_out,
|
||||
g.throttle_min.get(),
|
||||
g.throttle_max.get());
|
||||
}
|
||||
|
||||
if (control_mode >= AUTO) {
|
||||
@ -120,12 +93,12 @@ static void set_servos(void)
|
||||
#if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
|
||||
// send values to the PWM timers for output
|
||||
// ----------------------------------------
|
||||
hal.rcout->write(CH_1, g.channel_roll.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_2, g.channel_pitch.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_1, g.channel_steer.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_3, g.channel_throttle.radio_out); // send to Servos
|
||||
hal.rcout->write(CH_4, g.channel_rudder.radio_out); // send to Servos
|
||||
// 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_4.output_ch(CH_4);
|
||||
g.rc_5.output_ch(CH_5);
|
||||
g.rc_6.output_ch(CH_6);
|
||||
g.rc_7.output_ch(CH_7);
|
||||
|
||||
@ -46,7 +46,6 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
|
||||
case AUTO:
|
||||
case RTL:
|
||||
case GUIDED:
|
||||
case CIRCLE:
|
||||
base_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
// note that MAV_MODE_FLAG_AUTO_ENABLED does not match what
|
||||
// APM does in any mode, as that is defined as "system finds its own goal
|
||||
@ -142,6 +141,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
|
||||
|
||||
case AUTO:
|
||||
case RTL:
|
||||
case GUIDED:
|
||||
control_sensors_enabled |= (1<<10); // 3D angular rate control
|
||||
control_sensors_enabled |= (1<<11); // attitude stabilisation
|
||||
control_sensors_enabled |= (1<<12); // yaw position
|
||||
@ -235,7 +235,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan)
|
||||
int16_t bearing = nav_bearing / 100;
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
nav_roll / 1.0e2,
|
||||
nav_steer / 1.0e2,
|
||||
0,
|
||||
bearing,
|
||||
target_bearing / 100,
|
||||
@ -275,10 +275,10 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||
chan,
|
||||
millis(),
|
||||
0, // port 0
|
||||
10000 * g.channel_roll.norm_output(),
|
||||
10000 * g.channel_pitch.norm_output(),
|
||||
10000 * g.channel_steer.norm_output(),
|
||||
0,
|
||||
10000 * g.channel_throttle.norm_output(),
|
||||
10000 * g.channel_rudder.norm_output(),
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
@ -1068,7 +1068,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
|
||||
case LEARNING:
|
||||
case AUTO:
|
||||
case RTL:
|
||||
set_mode(packet.custom_mode);
|
||||
set_mode((enum mode)packet.custom_mode);
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -391,13 +391,10 @@ static void Log_Read_Startup()
|
||||
|
||||
struct log_Control_Tuning {
|
||||
LOG_PACKET_HEADER;
|
||||
int16_t roll_out;
|
||||
int16_t nav_roll_cd;
|
||||
int16_t steer_out;
|
||||
int16_t roll;
|
||||
int16_t pitch_out;
|
||||
int16_t pitch;
|
||||
int16_t throttle_out;
|
||||
int16_t rudder_out;
|
||||
int16_t accel_y;
|
||||
};
|
||||
|
||||
@ -408,14 +405,11 @@ static void Log_Write_Control_Tuning()
|
||||
Vector3f accel = ins.get_accel();
|
||||
struct log_Control_Tuning pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
|
||||
roll_out : (int16_t)g.channel_roll.servo_out,
|
||||
nav_roll_cd : (int16_t)nav_roll,
|
||||
steer_out : (int16_t)g.channel_steer.servo_out,
|
||||
roll : (int16_t)ahrs.roll_sensor,
|
||||
pitch_out : (int16_t)g.channel_pitch.servo_out,
|
||||
pitch : (int16_t)ahrs.pitch_sensor,
|
||||
throttle_out : (int16_t)g.channel_throttle.servo_out,
|
||||
rudder_out : (int16_t)g.channel_rudder.servo_out,
|
||||
accel_y : (int16_t)accel.y * 10000
|
||||
accel_y : (int16_t)(accel.y * 10000)
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
@ -427,14 +421,11 @@ static void Log_Read_Control_Tuning()
|
||||
struct log_Control_Tuning pkt;
|
||||
DataFlash.ReadPacket(&pkt, sizeof(pkt));
|
||||
|
||||
cliSerial->printf_P(PSTR("CTUN, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f\n"),
|
||||
(float)pkt.roll_out / 100.f,
|
||||
(float)pkt.nav_roll_cd / 100.f,
|
||||
cliSerial->printf_P(PSTR("CTUN, %4.2f, %4.2f, %4.2f, %4.2f, %4.2f\n"),
|
||||
(float)pkt.steer_out / 100.f,
|
||||
(float)pkt.roll / 100.f,
|
||||
(float)pkt.pitch_out / 100.f,
|
||||
(float)pkt.pitch / 100.f,
|
||||
(float)pkt.throttle_out / 100.f,
|
||||
(float)pkt.rudder_out / 100.f,
|
||||
(float)pkt.accel_y / 10000.f
|
||||
);
|
||||
}
|
||||
@ -459,7 +450,7 @@ static void Log_Write_Nav_Tuning()
|
||||
target_bearing_cd : (uint16_t)target_bearing,
|
||||
nav_bearing_cd : (uint16_t)nav_bearing,
|
||||
altitude_error_cm : (int16_t)altitude_error,
|
||||
nav_gain_scheduler : (int16_t)nav_gain_scaler*1000
|
||||
nav_gain_scheduler : (int16_t)(nav_gain_scaler*1000)
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
}
|
||||
@ -500,7 +491,7 @@ static void Log_Read_Mode()
|
||||
struct log_Mode pkt;
|
||||
DataFlash.ReadPacket(&pkt, sizeof(pkt));
|
||||
cliSerial->printf_P(PSTR("MOD,"));
|
||||
print_flight_mode(pkt.mode);
|
||||
print_mode(pkt.mode);
|
||||
}
|
||||
|
||||
struct log_GPS {
|
||||
|
||||
@ -34,17 +34,7 @@ public:
|
||||
|
||||
// Misc
|
||||
//
|
||||
k_param_auto_trim,
|
||||
k_param_switch_enable,
|
||||
k_param_log_bitmask,
|
||||
k_param_mix_mode, // unused
|
||||
k_param_reverse_elevons, // unused
|
||||
k_param_reverse_ch1_elevon, // unused
|
||||
k_param_reverse_ch2_elevon, // unused
|
||||
k_param_flap_1_percent, // unused
|
||||
k_param_flap_1_speed, // unused
|
||||
k_param_flap_2_percent, // unused
|
||||
k_param_flap_2_speed, // unused
|
||||
k_param_num_resets,
|
||||
k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change
|
||||
k_param_reset_switch_chan,
|
||||
@ -65,20 +55,10 @@ public:
|
||||
k_param_serial3_baud,
|
||||
k_param_telem_delay,
|
||||
|
||||
// 120: Fly-by-wire control
|
||||
//
|
||||
k_param_flybywire_airspeed_min = 120,
|
||||
k_param_flybywire_airspeed_max,
|
||||
k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
|
||||
|
||||
//
|
||||
// 130: Sensor parameters
|
||||
//
|
||||
k_param_imu = 130,
|
||||
k_param_altitude_mix, // sensor calibration
|
||||
k_param_airspeed_ratio,
|
||||
k_param_ground_temperature,
|
||||
k_param_ground_pressure,
|
||||
k_param_compass_enabled,
|
||||
k_param_compass,
|
||||
k_param_battery_monitoring,
|
||||
@ -86,13 +66,12 @@ public:
|
||||
k_param_curr_amp_per_volt,
|
||||
k_param_input_voltage,
|
||||
k_param_pack_capacity,
|
||||
k_param_airspeed_offset,
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
k_param_sonar_enabled,
|
||||
k_param_sonar_type,
|
||||
#endif
|
||||
#endif
|
||||
|
||||
k_param_ahrs, // AHRS group
|
||||
|
||||
//
|
||||
@ -100,19 +79,16 @@ public:
|
||||
//
|
||||
k_param_crosstrack_gain = 150,
|
||||
k_param_crosstrack_entry_angle,
|
||||
k_param_roll_limit,
|
||||
k_param_pitch_limit_max,
|
||||
k_param_pitch_limit_min,
|
||||
k_param_airspeed_cruise,
|
||||
k_param_min_gndspeed,
|
||||
k_param_speed_cruise,
|
||||
k_param_ch7_option,
|
||||
|
||||
//
|
||||
// 160: Radio settings
|
||||
//
|
||||
k_param_channel_roll = 160,
|
||||
k_param_channel_pitch,
|
||||
k_param_channel_steer = 160,
|
||||
k_param_rc_2,
|
||||
k_param_channel_throttle,
|
||||
k_param_channel_rudder,
|
||||
k_param_rc_4,
|
||||
k_param_rc_5,
|
||||
k_param_rc_6,
|
||||
k_param_rc_7,
|
||||
@ -124,39 +100,24 @@ public:
|
||||
k_param_throttle_fs_value,
|
||||
k_param_throttle_cruise,
|
||||
|
||||
k_param_short_fs_action,
|
||||
k_param_long_fs_action,
|
||||
k_param_gcs_heartbeat_fs_enabled,
|
||||
k_param_throttle_slewrate,
|
||||
|
||||
// ************************************************************
|
||||
// 180: APMrover parameters - JLN update
|
||||
|
||||
k_param_closed_loop_nav, // unused
|
||||
k_param_auto_wp_radius,
|
||||
k_param_sonar_trigger,
|
||||
k_param_turn_gain,
|
||||
k_param_booster,
|
||||
|
||||
// ************************************************************
|
||||
//
|
||||
// 200: Feed-forward gains
|
||||
//
|
||||
k_param_kff_pitch_compensation = 200, // unused
|
||||
k_param_kff_rudder_mix, // unused
|
||||
k_param_kff_pitch_to_throttle, // unused
|
||||
k_param_kff_throttle_to_pitch, // unused
|
||||
|
||||
//
|
||||
// 210: flight modes
|
||||
//
|
||||
k_param_flight_mode_channel = 210,
|
||||
k_param_flight_mode1,
|
||||
k_param_flight_mode2,
|
||||
k_param_flight_mode3,
|
||||
k_param_flight_mode4,
|
||||
k_param_flight_mode5,
|
||||
k_param_flight_mode6,
|
||||
k_param_mode_channel = 210,
|
||||
k_param_mode1,
|
||||
k_param_mode2,
|
||||
k_param_mode3,
|
||||
k_param_mode4,
|
||||
k_param_mode5,
|
||||
k_param_mode6,
|
||||
|
||||
//
|
||||
// 220: Waypoint data
|
||||
@ -165,69 +126,19 @@ public:
|
||||
k_param_command_total,
|
||||
k_param_command_index,
|
||||
k_param_waypoint_radius,
|
||||
k_param_loiter_radius, // unused
|
||||
k_param_fence_action,
|
||||
k_param_fence_total,
|
||||
k_param_fence_channel,
|
||||
k_param_fence_minalt,
|
||||
k_param_fence_maxalt,
|
||||
|
||||
// other objects
|
||||
k_param_sitl = 230,
|
||||
|
||||
//
|
||||
// 240: PID Controllers
|
||||
//
|
||||
// Heading-to-roll PID:
|
||||
// heading error from command to roll command deviation from trim
|
||||
// (bank to turn strategy)
|
||||
//
|
||||
k_param_pidNavRoll = 240,
|
||||
k_param_pidNavSteer = 240,
|
||||
|
||||
// Roll-to-servo PID:
|
||||
// roll error from command to roll servo deviation from trim
|
||||
// (tracks commanded bank angle)
|
||||
//
|
||||
k_param_pidServoRoll,
|
||||
// steering-to-servo PID:
|
||||
k_param_pidServoSteer,
|
||||
|
||||
//
|
||||
// Pitch control
|
||||
//
|
||||
// Pitch-to-servo PID:
|
||||
// pitch error from command to pitch servo deviation from trim
|
||||
// (front-side strategy)
|
||||
//
|
||||
k_param_pidServoPitch,
|
||||
|
||||
// Airspeed-to-pitch PID:
|
||||
// airspeed error from command to pitch servo deviation from trim
|
||||
// (back-side strategy)
|
||||
//
|
||||
k_param_pidNavPitchAirspeed,
|
||||
|
||||
//
|
||||
// Yaw control
|
||||
//
|
||||
// Yaw-to-servo PID:
|
||||
// yaw rate error from command to yaw servo deviation from trim
|
||||
// (stabilizes dutch roll)
|
||||
//
|
||||
k_param_pidServoRudder,
|
||||
|
||||
//
|
||||
// Throttle control
|
||||
//
|
||||
// Energy-to-throttle PID:
|
||||
// total energy error from command to throttle servo deviation from trim
|
||||
// (throttle back-side strategy alternative)
|
||||
//
|
||||
k_param_pidTeThrottle,
|
||||
|
||||
// Altitude-to-pitch PID:
|
||||
// altitude error from command to pitch servo deviation from trim
|
||||
// (throttle front-side strategy alternative)
|
||||
//
|
||||
k_param_pidNavPitchAltitude,
|
||||
// steering-to-servo PID:
|
||||
k_param_pidSpeedThrottle,
|
||||
|
||||
// 254,255: reserved
|
||||
};
|
||||
@ -248,24 +159,12 @@ public:
|
||||
AP_Float crosstrack_gain;
|
||||
AP_Int16 crosstrack_entry_angle;
|
||||
|
||||
// Estimation
|
||||
//
|
||||
AP_Float altitude_mix;
|
||||
AP_Float airspeed_ratio;
|
||||
AP_Int16 airspeed_offset;
|
||||
|
||||
// Waypoints
|
||||
//
|
||||
AP_Int8 waypoint_mode;
|
||||
AP_Int8 command_total;
|
||||
AP_Int8 command_index;
|
||||
AP_Int8 waypoint_radius;
|
||||
|
||||
// Fly-by-wire
|
||||
//
|
||||
AP_Int8 flybywire_airspeed_min;
|
||||
AP_Int8 flybywire_airspeed_max;
|
||||
AP_Int16 FBWB_min_altitude;
|
||||
AP_Float waypoint_radius;
|
||||
|
||||
// Throttle
|
||||
//
|
||||
@ -277,50 +176,38 @@ public:
|
||||
AP_Int8 throttle_cruise;
|
||||
|
||||
// Failsafe
|
||||
AP_Int8 short_fs_action;
|
||||
AP_Int8 long_fs_action;
|
||||
AP_Int8 gcs_heartbeat_fs_enabled;
|
||||
|
||||
// Flight modes
|
||||
//
|
||||
AP_Int8 flight_mode_channel;
|
||||
AP_Int8 flight_mode1;
|
||||
AP_Int8 flight_mode2;
|
||||
AP_Int8 flight_mode3;
|
||||
AP_Int8 flight_mode4;
|
||||
AP_Int8 flight_mode5;
|
||||
AP_Int8 flight_mode6;
|
||||
|
||||
// Navigational maneuvering limits
|
||||
//
|
||||
AP_Int16 roll_limit;
|
||||
AP_Int16 pitch_limit_max;
|
||||
AP_Int16 pitch_limit_min;
|
||||
AP_Int8 mode_channel;
|
||||
AP_Int8 mode1;
|
||||
AP_Int8 mode2;
|
||||
AP_Int8 mode3;
|
||||
AP_Int8 mode4;
|
||||
AP_Int8 mode5;
|
||||
AP_Int8 mode6;
|
||||
|
||||
// Misc
|
||||
//
|
||||
AP_Int8 auto_trim;
|
||||
AP_Int16 num_resets;
|
||||
AP_Int16 log_bitmask;
|
||||
AP_Int16 log_last_filenumber; // *** Deprecated - remove with next eeprom number change
|
||||
AP_Int8 reset_switch_chan;
|
||||
AP_Int8 manual_level;
|
||||
AP_Int16 airspeed_cruise;
|
||||
AP_Int16 min_gndspeed;
|
||||
AP_Float speed_cruise;
|
||||
AP_Int8 ch7_option;
|
||||
|
||||
AP_Int16 ground_temperature;
|
||||
AP_Int32 ground_pressure;
|
||||
AP_Int8 compass_enabled;
|
||||
AP_Int16 angle_of_attack;
|
||||
AP_Int8 battery_monitoring; // 0=disabled, 3=voltage only, 4=voltage and current
|
||||
AP_Float volt_div_ratio;
|
||||
AP_Float curr_amp_per_volt;
|
||||
AP_Float input_voltage;
|
||||
AP_Int16 pack_capacity; // Battery pack capacity less reserve
|
||||
AP_Int8 rssi_pin;
|
||||
AP_Int8 battery_volt_pin;
|
||||
AP_Int8 battery_curr_pin;
|
||||
AP_Int8 rssi_pin;
|
||||
AP_Int8 battery_volt_pin;
|
||||
AP_Int8 battery_curr_pin;
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if CONFIG_SONAR == ENABLED
|
||||
@ -334,18 +221,16 @@ public:
|
||||
// ************ ThermoPilot parameters ************************
|
||||
// - JLN update
|
||||
|
||||
AP_Int8 auto_wp_radius;
|
||||
AP_Int16 sonar_trigger;
|
||||
AP_Int16 turn_gain;
|
||||
AP_Float sonar_trigger;
|
||||
AP_Int8 booster;
|
||||
|
||||
// ************************************************************
|
||||
|
||||
// RC channels
|
||||
RC_Channel channel_roll;
|
||||
RC_Channel channel_pitch;
|
||||
RC_Channel channel_throttle;
|
||||
RC_Channel channel_rudder;
|
||||
RC_Channel channel_steer;
|
||||
RC_Channel_aux rc_2;
|
||||
RC_Channel channel_throttle;
|
||||
RC_Channel_aux rc_4;
|
||||
RC_Channel_aux rc_5;
|
||||
RC_Channel_aux rc_6;
|
||||
RC_Channel_aux rc_7;
|
||||
@ -353,20 +238,16 @@ public:
|
||||
|
||||
// PID controllers
|
||||
//
|
||||
PID pidNavRoll;
|
||||
PID pidServoRoll;
|
||||
PID pidServoPitch;
|
||||
PID pidNavPitchAirspeed;
|
||||
PID pidServoRudder;
|
||||
PID pidTeThrottle;
|
||||
PID pidNavPitchAltitude;
|
||||
PID pidNavSteer;
|
||||
PID pidServoSteer;
|
||||
PID pidSpeedThrottle;
|
||||
|
||||
Parameters() :
|
||||
// RC channels
|
||||
channel_roll(CH_1),
|
||||
channel_pitch(CH_2),
|
||||
channel_steer(CH_1),
|
||||
rc_2(CH_2),
|
||||
channel_throttle(CH_3),
|
||||
channel_rudder(CH_4),
|
||||
rc_4(CH_4),
|
||||
rc_5(CH_5),
|
||||
rc_6(CH_6),
|
||||
rc_7(CH_7),
|
||||
@ -374,13 +255,9 @@ public:
|
||||
|
||||
// PID controller initial P initial I initial D initial imax
|
||||
//-----------------------------------------------------------------------------------
|
||||
pidNavRoll (NAV_ROLL_P, NAV_ROLL_I, NAV_ROLL_D, NAV_ROLL_INT_MAX_CENTIDEGREE),
|
||||
pidServoRoll (SERVO_ROLL_P, SERVO_ROLL_I, SERVO_ROLL_D, SERVO_ROLL_INT_MAX_CENTIDEGREE),
|
||||
pidServoPitch (SERVO_PITCH_P, SERVO_PITCH_I, SERVO_PITCH_D, SERVO_PITCH_INT_MAX_CENTIDEGREE),
|
||||
pidNavPitchAirspeed (NAV_PITCH_ASP_P, NAV_PITCH_ASP_I, NAV_PITCH_ASP_D, NAV_PITCH_ASP_INT_MAX_CMSEC),
|
||||
pidServoRudder (SERVO_YAW_P, SERVO_YAW_I, SERVO_YAW_D, SERVO_YAW_INT_MAX),
|
||||
pidTeThrottle (THROTTLE_TE_P, THROTTLE_TE_I, THROTTLE_TE_D, THROTTLE_TE_INT_MAX),
|
||||
pidNavPitchAltitude (NAV_PITCH_ALT_P, NAV_PITCH_ALT_I, NAV_PITCH_ALT_D, NAV_PITCH_ALT_INT_MAX_CM)
|
||||
pidNavSteer (0.7, 0.1, 0.2, 200),
|
||||
pidServoSteer (0.5, 0.1, 0.2, 200),
|
||||
pidSpeedThrottle (0.7, 0.2, 0.2, 200)
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
@ -44,14 +44,14 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
|
||||
// @Param: MANUAL_LEVEL
|
||||
// @DisplayName: Manual Level
|
||||
// @Description: Setting this to Disabled(0) will enable autolevel on every boot. Setting it to Enabled(1) will do a calibration only when you tell it to
|
||||
// @Description: Setting this to Disabled(0) will enable autolevel on every boot. Setting it to Enabled(1) will do a calibration only when you tell it to. This defaults to enabled.
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Advanced
|
||||
GSCALAR(manual_level, "MANUAL_LEVEL", 0),
|
||||
GSCALAR(manual_level, "MANUAL_LEVEL", 1),
|
||||
|
||||
// @Param: XTRK_GAIN_SC
|
||||
// @DisplayName: Crosstrack Gain
|
||||
// @Description: The scale between distance off the line and angle to meet the line (in Degrees * 100)
|
||||
// @Description: This controls how hard the Rover tries to follow the lines between waypoints, as opposed to driving directly to the next waypoint. The value is the scale between distance off the line and angle to meet the line (in Degrees * 100)
|
||||
// @Range: 0 2000
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
@ -68,11 +68,19 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
|
||||
GSCALAR(command_total, "CMD_TOTAL", 0),
|
||||
GSCALAR(command_index, "CMD_INDEX", 0),
|
||||
GSCALAR(waypoint_radius, "WP_RADIUS", WP_RADIUS_DEFAULT),
|
||||
|
||||
// @Param: WP_RADIUS
|
||||
// @DisplayName: Waypoint radius
|
||||
// @Description: The distance in meters from a waypoint when we consider the waypoint has been reached. This determines when the rover will turn along the next waypoint path.
|
||||
// @Units: meters
|
||||
// @Range: 0 1000
|
||||
// @Increment: 0.1
|
||||
// @User: Standard
|
||||
GSCALAR(waypoint_radius, "WP_RADIUS", 2.0f),
|
||||
|
||||
// @Param: THR_MIN
|
||||
// @DisplayName: Minimum Throttle
|
||||
// @Description: The minimum throttle setting to which the autopilot will apply.
|
||||
// @Description: The minimum throttle setting to which the autopilot will apply. This is mostly useful for rovers with internal combustion motors, to prevent the motor from cutting out in auto mode.
|
||||
// @Units: Percent
|
||||
// @Range: 0 100
|
||||
// @Increment: 1
|
||||
@ -95,110 +103,98 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @Range: 0 100
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(throttle_slewrate, "THR_SLEWRATE", THROTTLE_SLEW_LIMIT),
|
||||
GSCALAR(throttle_slewrate, "THR_SLEWRATE", 0),
|
||||
|
||||
// @Param: THR_FAILSAFE
|
||||
// @DisplayName: Throttle Failsafe Enable
|
||||
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
|
||||
// @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel to a low value. This can be used to detect the RC transmitter going out of range.
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
GSCALAR(throttle_fs_enabled, "THR_FAILSAFE", THROTTLE_FAILSAFE),
|
||||
|
||||
// @Param: THR_FS_VALUE
|
||||
// @DisplayName: Throttle Failsafe Value
|
||||
// @Description: The PWM level on channel 3 below which throttle sailsafe triggers
|
||||
// @Description: The PWM level on channel 3 below which throttle sailsafe triggers.
|
||||
// @User: Standard
|
||||
GSCALAR(throttle_fs_value, "THR_FS_VALUE", THROTTLE_FS_VALUE),
|
||||
|
||||
// @Param: TRIM_THROTTLE
|
||||
// @Param: CRUISE_THROTTLE
|
||||
// @DisplayName: Throttle cruise percentage
|
||||
// @Description: The target percentage of throttle to apply for normal flight
|
||||
// @Description: The target percentage of throttle to apply for auto missions.
|
||||
// @Units: Percent
|
||||
// @Range: 0 100
|
||||
// @Increment: 1
|
||||
// @User: Standard
|
||||
GSCALAR(throttle_cruise, "TRIM_THROTTLE", THROTTLE_CRUISE),
|
||||
GSCALAR(throttle_cruise, "CRUISE_THROTTLE", 50),
|
||||
|
||||
// @Param: FS_SHORT_ACTN
|
||||
// @DisplayName: Short failsafe action
|
||||
// @Description: The action to take on a short (1 second) failsafe event
|
||||
// @Values: 0:None,1:ReturnToLaunch
|
||||
// @Param: CRUISE_SPEED
|
||||
// @DisplayName: Target speed in auto modes
|
||||
// @Description: The target speed in auto missions.
|
||||
// @Units: m/s
|
||||
// @Range: 0 100
|
||||
// @Increment: 0.1
|
||||
// @User: Standard
|
||||
GSCALAR(short_fs_action, "FS_SHORT_ACTN", SHORT_FAILSAFE_ACTION),
|
||||
GSCALAR(speed_cruise, "CRUISE_SPEED", 5),
|
||||
|
||||
// @Param: FS_LONG_ACTN
|
||||
// @DisplayName: Long failsafe action
|
||||
// @Description: The action to take on a long (20 second) failsafe event
|
||||
// @Values: 0:None,1:ReturnToLaunch
|
||||
// @User: Standard
|
||||
GSCALAR(long_fs_action, "FS_LONG_ACTN", LONG_FAILSAFE_ACTION),
|
||||
|
||||
// @Param: FS_GCS_ENABL
|
||||
// @Param: FS_GCS_ENABLE
|
||||
// @DisplayName: GCS failsafe enable
|
||||
// @Description: Enable ground control station telemetry failsafe. Failsafe will trigger after 20 seconds of no MAVLink heartbeat messages
|
||||
// @Description: Enable ground control station telemetry failsafe
|
||||
// @Values: 0:Disabled,1:Enabled
|
||||
// @User: Standard
|
||||
GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABL", GCS_HEARTBEAT_FAILSAFE),
|
||||
GSCALAR(gcs_heartbeat_fs_enabled, "FS_GCS_ENABLE", 0),
|
||||
|
||||
// @Param: FLTMODE_CH
|
||||
// @DisplayName: Flightmode channel
|
||||
// @Param: MODE_CH
|
||||
// @DisplayName: Mode channel
|
||||
// @Description: RC Channel to use for flight mode control
|
||||
// @User: Advanced
|
||||
GSCALAR(flight_mode_channel, "FLTMODE_CH", FLIGHT_MODE_CHANNEL),
|
||||
GSCALAR(mode_channel, "MODE_CH", MODE_CHANNEL),
|
||||
|
||||
// @Param: FLTMODE1
|
||||
// @DisplayName: FlightMode1
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Param: MODE1
|
||||
// @DisplayName: Mode1
|
||||
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
|
||||
// @User: Standard
|
||||
// @Description: Flight mode for switch position 1 (910 to 1230 and above 2049)
|
||||
GSCALAR(flight_mode1, "FLTMODE1", FLIGHT_MODE_1),
|
||||
GSCALAR(mode1, "MODE1", MODE_1),
|
||||
|
||||
// @Param: FLTMODE2
|
||||
// @DisplayName: FlightMode2
|
||||
// @Param: MODE2
|
||||
// @DisplayName: Mode2
|
||||
// @Description: Flight mode for switch position 2 (1231 to 1360)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode2, "FLTMODE2", FLIGHT_MODE_2),
|
||||
GSCALAR(mode2, "MODE2", MODE_2),
|
||||
|
||||
// @Param: FLTMODE3
|
||||
// @DisplayName: FlightMode3
|
||||
// @Param: MODE3
|
||||
// @DisplayName: Mode3
|
||||
// @Description: Flight mode for switch position 3 (1361 to 1490)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode3, "FLTMODE3", FLIGHT_MODE_3),
|
||||
GSCALAR(mode3, "MODE3", MODE_3),
|
||||
|
||||
// @Param: FLTMODE4
|
||||
// @DisplayName: FlightMode4
|
||||
// @Param: MODE4
|
||||
// @DisplayName: Mode4
|
||||
// @Description: Flight mode for switch position 4 (1491 to 1620)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode4, "FLTMODE4", FLIGHT_MODE_4),
|
||||
GSCALAR(mode4, "MODE4", MODE_4),
|
||||
|
||||
// @Param: FLTMODE5
|
||||
// @DisplayName: FlightMode5
|
||||
// @Param: MODE5
|
||||
// @DisplayName: Mode5
|
||||
// @Description: Flight mode for switch position 5 (1621 to 1749)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode5, "FLTMODE5", FLIGHT_MODE_5),
|
||||
GSCALAR(mode5, "MODE5", MODE_5),
|
||||
|
||||
// @Param: FLTMODE6
|
||||
// @DisplayName: FlightMode6
|
||||
// @Param: MODE6
|
||||
// @DisplayName: Mode6
|
||||
// @Description: Flight mode for switch position 6 (1750 to 2049)
|
||||
// @Values: 0:Manual,1:CIRCLE,2:STABILIZE,5:FBWA,6:FBWB,10:Auto,11:RTL,12:Loiter,15:Guided
|
||||
// @Values: 0:Manual,2:LEARNING,10:Auto,11:RTL,15:Guided
|
||||
// @User: Standard
|
||||
GSCALAR(flight_mode6, "FLTMODE6", FLIGHT_MODE_6),
|
||||
GSCALAR(mode6, "MODE6", MODE_6),
|
||||
|
||||
GSCALAR(roll_limit, "LIM_ROLL_CD", HEAD_MAX_CENTIDEGREE),
|
||||
GSCALAR(pitch_limit_max, "LIM_PITCH_MAX", PITCH_MAX_CENTIDEGREE),
|
||||
GSCALAR(pitch_limit_min, "LIM_PITCH_MIN", PITCH_MIN_CENTIDEGREE),
|
||||
|
||||
GSCALAR(auto_trim, "TRIM_AUTO", AUTO_TRIM),
|
||||
GSCALAR(num_resets, "SYS_NUM_RESETS", 0),
|
||||
GSCALAR(log_bitmask, "LOG_BITMASK", DEFAULT_LOG_BITMASK),
|
||||
GSCALAR(log_last_filenumber, "LOG_LASTFILE", 0),
|
||||
GSCALAR(reset_switch_chan, "RST_SWITCH_CH", 0),
|
||||
GSCALAR(airspeed_cruise, "TRIM_ARSPD_CM", AIRSPEED_CRUISE_CM),
|
||||
GSCALAR(min_gndspeed, "MIN_GNDSPD_CM", MIN_GNDSPEED_CM),
|
||||
GSCALAR(ch7_option, "CH7_OPT", CH7_OPTION),
|
||||
|
||||
GSCALAR(compass_enabled, "MAG_ENABLE", MAGNETOMETER),
|
||||
@ -244,29 +240,23 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
|
||||
// ************************************************************
|
||||
// APMrover parameters - JLN update
|
||||
GSCALAR(auto_wp_radius, "ROV_AWPR_NAV", AUTO_WP_RADIUS),
|
||||
GSCALAR(sonar_trigger, "ROV_SONAR_TRIG", SONAR_TRIGGER),
|
||||
GSCALAR(turn_gain, "ROV_GAIN", TURN_GAIN),
|
||||
GSCALAR(booster, "ROV_BOOSTER", BOOSTER),
|
||||
|
||||
// ************************************************************
|
||||
|
||||
GGROUP(channel_roll, "RC1_", RC_Channel),
|
||||
GGROUP(channel_pitch, "RC2_", RC_Channel),
|
||||
GGROUP(channel_steer, "RC1_", RC_Channel),
|
||||
GGROUP(rc_2, "RC2_", RC_Channel_aux),
|
||||
GGROUP(channel_throttle, "RC3_", RC_Channel),
|
||||
GGROUP(channel_rudder, "RC4_", RC_Channel),
|
||||
GGROUP(rc_4, "RC4_", RC_Channel_aux),
|
||||
GGROUP(rc_5, "RC5_", RC_Channel_aux),
|
||||
GGROUP(rc_6, "RC6_", RC_Channel_aux),
|
||||
GGROUP(rc_7, "RC7_", RC_Channel_aux),
|
||||
GGROUP(rc_8, "RC8_", RC_Channel_aux),
|
||||
|
||||
GGROUP(pidNavRoll, "HDNG2RLL_", PID),
|
||||
GGROUP(pidServoRoll, "RLL2SRV_", PID),
|
||||
GGROUP(pidServoPitch, "PTCH2SRV_", PID),
|
||||
GGROUP(pidNavPitchAirspeed, "ARSP2PTCH_", PID),
|
||||
GGROUP(pidServoRudder, "YW2SRV_", PID),
|
||||
GGROUP(pidTeThrottle, "ENRGY2THR_", PID),
|
||||
GGROUP(pidNavPitchAltitude, "ALT2PTCH_", PID),
|
||||
GGROUP(pidNavSteer, "HDNG2STEER_", PID),
|
||||
GGROUP(pidServoSteer, "STEER2SRV_", PID),
|
||||
GGROUP(pidSpeedThrottle, "SPEED2THR_", PID),
|
||||
|
||||
// variables not in the g class which contain EEPROM saved variables
|
||||
GOBJECT(compass, "COMPASS_", Compass),
|
||||
|
||||
@ -194,13 +194,6 @@ static bool verify_takeoff()
|
||||
{ return true;
|
||||
}
|
||||
|
||||
static void calc_turn_radius(void) // JLN update - adjut automaticaly the wp_radius Vs the speed and the turn angle
|
||||
{
|
||||
wp_radius = ground_speed * 150 / g.roll_limit.get();
|
||||
//cliSerial->println(wp_radius, DEC);
|
||||
}
|
||||
|
||||
|
||||
static bool verify_nav_wp()
|
||||
{
|
||||
update_crosstrack();
|
||||
@ -212,15 +205,6 @@ static bool verify_nav_wp()
|
||||
return true;
|
||||
}
|
||||
|
||||
if(g.auto_wp_radius) {
|
||||
calc_turn_radius(); // JLN update - auto-adap the wp_radius Vs the gspeed and max roll angle
|
||||
|
||||
if ((wp_distance > 0) && (wp_distance <= wp_radius)) {
|
||||
gcs_send_text_fmt(PSTR("Reached Waypoint #%i"),nav_command_index);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// have we gone past the waypoint?
|
||||
if (location_passed_point(current_loc, prev_WP, next_WP)) {
|
||||
gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"),
|
||||
@ -335,12 +319,9 @@ static void do_change_speed()
|
||||
{
|
||||
switch (next_nonnav_command.p1)
|
||||
{
|
||||
case 0: // Airspeed
|
||||
if(next_nonnav_command.alt > 0)
|
||||
g.airspeed_cruise.set(next_nonnav_command.alt * 100);
|
||||
break;
|
||||
case 1: // Ground speed
|
||||
g.min_gndspeed.set(next_nonnav_command.alt * 100);
|
||||
case 0:
|
||||
if (next_nonnav_command.alt > 0)
|
||||
g.speed_cruise.set(next_nonnav_command.alt * 100);
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
@ -226,7 +226,7 @@
|
||||
#endif
|
||||
|
||||
#ifndef CH7_OPTION
|
||||
# define CH7_OPTION CH7_DO_NOTHING
|
||||
# define CH7_OPTION CH7_SAVE_WP
|
||||
#endif
|
||||
|
||||
#ifndef TUNING_OPTION
|
||||
@ -273,94 +273,47 @@
|
||||
# define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// RADIO CONFIGURATION
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Radio channel limits
|
||||
// MODE
|
||||
// MODE_CHANNEL
|
||||
//
|
||||
// Note that these are not called out in APM_Config.h.reference.
|
||||
//
|
||||
#ifndef CH5_MIN
|
||||
# define CH5_MIN 1000
|
||||
#ifndef MODE_CHANNEL
|
||||
# define MODE_CHANNEL 8
|
||||
#endif
|
||||
#ifndef CH5_MAX
|
||||
# define CH5_MAX 2000
|
||||
#endif
|
||||
#ifndef CH6_MIN
|
||||
# define CH6_MIN 1000
|
||||
#endif
|
||||
#ifndef CH6_MAX
|
||||
# define CH6_MAX 2000
|
||||
#endif
|
||||
#ifndef CH7_MIN
|
||||
# define CH7_MIN 1000
|
||||
#endif
|
||||
#ifndef CH7_MAX
|
||||
# define CH7_MAX 2000
|
||||
#endif
|
||||
#ifndef CH8_MIN
|
||||
# define CH8_MIN 1000
|
||||
#endif
|
||||
#ifndef CH8_MAX
|
||||
# define CH8_MAX 2000
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// FLIGHT_MODE
|
||||
// FLIGHT_MODE_CHANNEL
|
||||
//
|
||||
#ifndef FLIGHT_MODE_CHANNEL
|
||||
# define FLIGHT_MODE_CHANNEL 8
|
||||
#endif
|
||||
#if (FLIGHT_MODE_CHANNEL != 5) && (FLIGHT_MODE_CHANNEL != 6) && (FLIGHT_MODE_CHANNEL != 7) && (FLIGHT_MODE_CHANNEL != 8)
|
||||
#if (MODE_CHANNEL != 5) && (MODE_CHANNEL != 6) && (MODE_CHANNEL != 7) && (MODE_CHANNEL != 8)
|
||||
# error XXX
|
||||
# error XXX You must set FLIGHT_MODE_CHANNEL to 5, 6, 7 or 8
|
||||
# error XXX You must set MODE_CHANNEL to 5, 6, 7 or 8
|
||||
# error XXX
|
||||
#endif
|
||||
|
||||
#if !defined(FLIGHT_MODE_1)
|
||||
# define FLIGHT_MODE_1 LEARNING
|
||||
#if !defined(MODE_1)
|
||||
# define MODE_1 LEARNING
|
||||
#endif
|
||||
#if !defined(FLIGHT_MODE_2)
|
||||
# define FLIGHT_MODE_2 LEARNING
|
||||
#if !defined(MODE_2)
|
||||
# define MODE_2 LEARNING
|
||||
#endif
|
||||
#if !defined(FLIGHT_MODE_3)
|
||||
# define FLIGHT_MODE_3 LEARNING
|
||||
#if !defined(MODE_3)
|
||||
# define MODE_3 LEARNING
|
||||
#endif
|
||||
#if !defined(FLIGHT_MODE_4)
|
||||
# define FLIGHT_MODE_4 LEARNING
|
||||
#if !defined(MODE_4)
|
||||
# define MODE_4 LEARNING
|
||||
#endif
|
||||
#if !defined(FLIGHT_MODE_5)
|
||||
# define FLIGHT_MODE_5 LEARNING
|
||||
#if !defined(MODE_5)
|
||||
# define MODE_5 LEARNING
|
||||
#endif
|
||||
#if !defined(FLIGHT_MODE_6)
|
||||
# define FLIGHT_MODE_6 MANUAL
|
||||
#if !defined(MODE_6)
|
||||
# define MODE_6 MANUAL
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// THROTTLE_FAILSAFE
|
||||
// THROTTLE_FS_VALUE
|
||||
// SHORT_FAILSAFE_ACTION
|
||||
// LONG_FAILSAFE_ACTION
|
||||
// GCS_HEARTBEAT_FAILSAFE
|
||||
//
|
||||
// failsafe defaults
|
||||
#ifndef THROTTLE_FAILSAFE
|
||||
# define THROTTLE_FAILSAFE ENABLED
|
||||
#endif
|
||||
#ifndef THROTTLE_FS_VALUE
|
||||
# define THROTTLE_FS_VALUE 950
|
||||
#endif
|
||||
#ifndef SHORT_FAILSAFE_ACTION
|
||||
# define SHORT_FAILSAFE_ACTION 0
|
||||
#endif
|
||||
#ifndef LONG_FAILSAFE_ACTION
|
||||
# define LONG_FAILSAFE_ACTION 0
|
||||
#endif
|
||||
@ -369,29 +322,6 @@
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// AUTO_TRIM
|
||||
//
|
||||
#ifndef AUTO_TRIM
|
||||
# define AUTO_TRIM DISABLED
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// MANUAL_LEVEL
|
||||
//
|
||||
#ifndef MANUAL_LEVEL
|
||||
# define MANUAL_LEVEL DISABLED
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// ENABLE_STICK_MIXING
|
||||
//
|
||||
#ifndef ENABLE_STICK_MIXING
|
||||
# define ENABLE_STICK_MIXING ENABLED
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// THROTTLE_OUT
|
||||
//
|
||||
@ -423,10 +353,9 @@
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// AIRSPEED_CRUISE
|
||||
//
|
||||
#ifndef AIRSPEED_CRUISE
|
||||
# define AIRSPEED_CRUISE 3 // 12 m/s
|
||||
#ifndef SPEED_CRUISE
|
||||
# define SPEED_CRUISE 3 // 3 m/s
|
||||
#endif
|
||||
#define AIRSPEED_CRUISE_CM AIRSPEED_CRUISE*100
|
||||
|
||||
#ifndef GSBOOST
|
||||
# define GSBOOST 0
|
||||
@ -446,14 +375,6 @@
|
||||
# define TURN_GAIN 5
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// MIN_GNDSPEED
|
||||
//
|
||||
#ifndef MIN_GNDSPEED
|
||||
# define MIN_GNDSPEED 0 // m/s (0 disables)
|
||||
#endif
|
||||
#define MIN_GNDSPEED_CM MIN_GNDSPEED*100
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Servo Mapping
|
||||
//
|
||||
@ -467,146 +388,24 @@
|
||||
# define THROTTLE_MAX 75
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Autopilot control limits
|
||||
//
|
||||
#ifndef HEAD_MAX
|
||||
# define HEAD_MAX 45
|
||||
#endif
|
||||
#ifndef PITCH_MAX
|
||||
# define PITCH_MAX 15
|
||||
#endif
|
||||
#ifndef PITCH_MIN
|
||||
# define PITCH_MIN -25
|
||||
#endif
|
||||
#define HEAD_MAX_CENTIDEGREE HEAD_MAX * 100
|
||||
#define PITCH_MAX_CENTIDEGREE PITCH_MAX * 100
|
||||
#define PITCH_MIN_CENTIDEGREE PITCH_MIN * 100
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Attitude control gains
|
||||
//
|
||||
#ifndef SERVO_ROLL_P
|
||||
# define SERVO_ROLL_P 0.4
|
||||
#ifndef SERVO_STEER_P
|
||||
# define SERVO_STEER_P 0.4
|
||||
#endif
|
||||
#ifndef SERVO_ROLL_I
|
||||
# define SERVO_ROLL_I 0.0
|
||||
#ifndef SERVO_STEER_I
|
||||
# define SERVO_STEER_I 0.0
|
||||
#endif
|
||||
#ifndef SERVO_ROLL_D
|
||||
# define SERVO_ROLL_D 0.0
|
||||
#ifndef SERVO_STEER_D
|
||||
# define SERVO_STEER_D 0.0
|
||||
#endif
|
||||
#ifndef SERVO_ROLL_INT_MAX
|
||||
# define SERVO_ROLL_INT_MAX 5
|
||||
#endif
|
||||
#define SERVO_ROLL_INT_MAX_CENTIDEGREE SERVO_ROLL_INT_MAX*100
|
||||
#ifndef ROLL_SLEW_LIMIT
|
||||
# define ROLL_SLEW_LIMIT 0
|
||||
#endif
|
||||
#ifndef SERVO_PITCH_P
|
||||
# define SERVO_PITCH_P 0.6
|
||||
#endif
|
||||
#ifndef SERVO_PITCH_I
|
||||
# define SERVO_PITCH_I 0.0
|
||||
#endif
|
||||
#ifndef SERVO_PITCH_D
|
||||
# define SERVO_PITCH_D 0.0
|
||||
#endif
|
||||
#ifndef SERVO_PITCH_INT_MAX
|
||||
# define SERVO_PITCH_INT_MAX 5
|
||||
#endif
|
||||
#define SERVO_PITCH_INT_MAX_CENTIDEGREE SERVO_PITCH_INT_MAX*100
|
||||
#ifndef PITCH_COMP
|
||||
# define PITCH_COMP 0.2
|
||||
#endif
|
||||
#ifndef SERVO_YAW_P
|
||||
# define SERVO_YAW_P 0.0
|
||||
#endif
|
||||
#ifndef SERVO_YAW_I
|
||||
# define SERVO_YAW_I 0.0
|
||||
#endif
|
||||
#ifndef SERVO_YAW_D
|
||||
# define SERVO_YAW_D 0.0
|
||||
#endif
|
||||
#ifndef SERVO_YAW_INT_MAX
|
||||
# define SERVO_YAW_INT_MAX 0
|
||||
#endif
|
||||
#ifndef RUDDER_MIX
|
||||
# define RUDDER_MIX 0.5
|
||||
#ifndef SERVO_STEER_INT_MAX
|
||||
# define SERVO_STEER_INT_MAX 5
|
||||
#endif
|
||||
#define SERVO_STEER_INT_MAX_CENTIDEGREE SERVO_STEER_INT_MAX*100
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation control gains
|
||||
//
|
||||
#ifndef NAV_ROLL_P
|
||||
# define NAV_ROLL_P 0.7
|
||||
#endif
|
||||
#ifndef NAV_ROLL_I
|
||||
# define NAV_ROLL_I 0.0
|
||||
#endif
|
||||
#ifndef NAV_ROLL_D
|
||||
# define NAV_ROLL_D 0.02
|
||||
#endif
|
||||
#ifndef NAV_ROLL_INT_MAX
|
||||
# define NAV_ROLL_INT_MAX 5
|
||||
#endif
|
||||
#define NAV_ROLL_INT_MAX_CENTIDEGREE NAV_ROLL_INT_MAX*100
|
||||
#ifndef NAV_PITCH_ASP_P
|
||||
# define NAV_PITCH_ASP_P 0.65
|
||||
#endif
|
||||
#ifndef NAV_PITCH_ASP_I
|
||||
# define NAV_PITCH_ASP_I 0.0
|
||||
#endif
|
||||
#ifndef NAV_PITCH_ASP_D
|
||||
# define NAV_PITCH_ASP_D 0.0
|
||||
#endif
|
||||
#ifndef NAV_PITCH_ASP_INT_MAX
|
||||
# define NAV_PITCH_ASP_INT_MAX 5
|
||||
#endif
|
||||
#define NAV_PITCH_ASP_INT_MAX_CMSEC NAV_PITCH_ASP_INT_MAX*100
|
||||
#ifndef NAV_PITCH_ALT_P
|
||||
# define NAV_PITCH_ALT_P 0.65
|
||||
#endif
|
||||
#ifndef NAV_PITCH_ALT_I
|
||||
# define NAV_PITCH_ALT_I 0.0
|
||||
#endif
|
||||
#ifndef NAV_PITCH_ALT_D
|
||||
# define NAV_PITCH_ALT_D 0.0
|
||||
#endif
|
||||
#ifndef NAV_PITCH_ALT_INT_MAX
|
||||
# define NAV_PITCH_ALT_INT_MAX 5
|
||||
#endif
|
||||
#define NAV_PITCH_ALT_INT_MAX_CM NAV_PITCH_ALT_INT_MAX*100
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Energy/Altitude control gains
|
||||
//
|
||||
#ifndef THROTTLE_TE_P
|
||||
# define THROTTLE_TE_P 0.50
|
||||
#endif
|
||||
#ifndef THROTTLE_TE_I
|
||||
# define THROTTLE_TE_I 0.0
|
||||
#endif
|
||||
#ifndef THROTTLE_TE_D
|
||||
# define THROTTLE_TE_D 0.0
|
||||
#endif
|
||||
#ifndef THROTTLE_TE_INT_MAX
|
||||
# define THROTTLE_TE_INT_MAX 20
|
||||
#endif
|
||||
#ifndef THROTTLE_SLEW_LIMIT
|
||||
# define THROTTLE_SLEW_LIMIT 0
|
||||
#endif
|
||||
#ifndef P_TO_T
|
||||
# define P_TO_T 0
|
||||
#endif
|
||||
#ifndef T_TO_P
|
||||
# define T_TO_P 0
|
||||
#endif
|
||||
#ifndef PITCH_TARGET
|
||||
# define PITCH_TARGET 0
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Crosstrack compensation
|
||||
//
|
||||
@ -619,21 +418,13 @@
|
||||
# define XTRACK_GAIN_SCALED XTRACK_GAIN*100
|
||||
# define XTRACK_ENTRY_ANGLE_CENTIDEGREE XTRACK_ENTRY_ANGLE*100
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// DEBUGGING
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Dataflash logging control
|
||||
//
|
||||
|
||||
#ifndef LOGGING_ENABLED
|
||||
# define LOGGING_ENABLED ENABLED
|
||||
#endif
|
||||
|
||||
|
||||
#ifndef LOG_ATTITUDE_FAST
|
||||
# define LOG_ATTITUDE_FAST DISABLED
|
||||
#endif
|
||||
@ -681,13 +472,6 @@
|
||||
LOGBIT(CURRENT)
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Navigation defaults
|
||||
//
|
||||
#ifndef WP_RADIUS_DEFAULT
|
||||
# define WP_RADIUS_DEFAULT 2
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Developer Items
|
||||
//
|
||||
@ -703,20 +487,11 @@
|
||||
# define HIL_SERVOS DISABLED
|
||||
#endif
|
||||
|
||||
#ifndef TRACE
|
||||
# define TRACE DISABLED
|
||||
#endif
|
||||
|
||||
// use this to completely disable the CLI
|
||||
#ifndef CLI_ENABLED
|
||||
# define CLI_ENABLED ENABLED
|
||||
#endif
|
||||
|
||||
// use this to disable the CLI slider switch
|
||||
#ifndef CLI_SLIDER_ENABLED
|
||||
# define CLI_SLIDER_ENABLED ENABLED
|
||||
#endif
|
||||
|
||||
// if RESET_SWITCH_CH is not zero, then this is the PWM value on
|
||||
// that channel where we reset the control mode to the current switch
|
||||
// position (to for example return to switched mode after failsafe or
|
||||
|
||||
@ -19,7 +19,7 @@ static void read_control_switch()
|
||||
(g.reset_switch_chan != 0 &&
|
||||
hal.rcin->read(g.reset_switch_chan-1) > RESET_SWITCH_CHAN_PWM)) {
|
||||
|
||||
set_mode(flight_modes[switchPosition]);
|
||||
set_mode((enum mode)modes[switchPosition].get());
|
||||
|
||||
oldSwitchPosition = switchPosition;
|
||||
prev_WP = current_loc;
|
||||
@ -33,7 +33,7 @@ static void read_control_switch()
|
||||
}
|
||||
|
||||
static uint8_t readSwitch(void){
|
||||
uint16_t pulsewidth = hal.rcin->read(g.flight_mode_channel - 1);
|
||||
uint16_t pulsewidth = hal.rcin->read(g.mode_channel - 1);
|
||||
if (pulsewidth <= 910 || pulsewidth >= 2090) return 255; // This is an error condition
|
||||
if (pulsewidth > 1230 && pulsewidth <= 1360) return 1;
|
||||
if (pulsewidth > 1360 && pulsewidth <= 1490) return 2;
|
||||
@ -55,29 +55,35 @@ static void reset_control_switch()
|
||||
// set this to your trainer switch
|
||||
static void read_trim_switch()
|
||||
{
|
||||
if (g.ch7_option == CH7_SAVE_WP) { // set to 1
|
||||
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER){ // switch is engaged
|
||||
trim_flag = true;
|
||||
switch ((enum ch7_option)g.ch7_option.get()) {
|
||||
case CH7_DO_NOTHING:
|
||||
break;
|
||||
case CH7_SAVE_WP:
|
||||
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
|
||||
// switch is engaged
|
||||
ch7_flag = true;
|
||||
} else { // switch is disengaged
|
||||
if(trim_flag) {
|
||||
trim_flag = false;
|
||||
if (ch7_flag) {
|
||||
ch7_flag = false;
|
||||
|
||||
if (control_mode == MANUAL) {
|
||||
hal.console->println_P(PSTR("Erasing waypoints"));
|
||||
// if SW7 is ON in MANUAL = Erase the Flight Plan
|
||||
CH7_wp_index = 0;
|
||||
g.command_total.set_and_save(CH7_wp_index);
|
||||
g.command_total = 0;
|
||||
g.command_index =0;
|
||||
nav_command_index = 0;
|
||||
if(g.channel_roll.control_in > 3000) // if roll is full right store the current location as home
|
||||
ground_start_count = 5;
|
||||
if (g.channel_steer.control_in > 3000) {
|
||||
// if roll is full right store the current location as home
|
||||
init_home();
|
||||
}
|
||||
CH7_wp_index = 1;
|
||||
return;
|
||||
} else if (control_mode == LEARNING) {
|
||||
// if SW7 is ON in LEARNING = record the Wp
|
||||
// set the next_WP (home is stored at 0)
|
||||
CH7_wp_index = constrain_int16(CH7_wp_index, 1, MAX_WAYPOINTS);
|
||||
|
||||
hal.console->printf_P(PSTR("Learning waypoint %u"), (unsigned)CH7_wp_index);
|
||||
current_loc.id = MAV_CMD_NAV_WAYPOINT;
|
||||
|
||||
// store the index
|
||||
@ -91,6 +97,7 @@ static void read_trim_switch()
|
||||
|
||||
// increment index
|
||||
CH7_wp_index++;
|
||||
CH7_wp_index = constrain_int16(CH7_wp_index, 1, MAX_WAYPOINTS);
|
||||
|
||||
} else if (control_mode == AUTO) {
|
||||
// if SW7 is ON in AUTO = set to RTL
|
||||
@ -98,19 +105,7 @@ static void read_trim_switch()
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (g.ch7_option == CH7_RTL) {
|
||||
// set to 6
|
||||
if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) {
|
||||
// switch is engaged
|
||||
trim_flag = true;
|
||||
} else { // switch is disengaged
|
||||
if (trim_flag) {
|
||||
trim_flag = false;
|
||||
if (control_mode == LEARNING) {
|
||||
set_mode(RTL);
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -33,9 +33,10 @@
|
||||
#define SONAR_SOURCE_ANALOG_PIN 2
|
||||
|
||||
// CH 7 control
|
||||
#define CH7_DO_NOTHING 0
|
||||
#define CH7_SAVE_WP 1
|
||||
#define CH7_RTL 6
|
||||
enum ch7_option {
|
||||
CH7_DO_NOTHING=0,
|
||||
CH7_SAVE_WP=1
|
||||
};
|
||||
|
||||
#define T6 1000000
|
||||
#define T7 10000000
|
||||
@ -66,15 +67,14 @@
|
||||
|
||||
// Auto Pilot modes
|
||||
// ----------------
|
||||
#define MANUAL 0
|
||||
#define CIRCLE 1 // When flying sans GPS, and we loose the radio, just circle
|
||||
#define LEARNING 2
|
||||
|
||||
#define AUTO 10
|
||||
#define RTL 11
|
||||
#define GUIDED 15
|
||||
#define INITIALISING 16 // in startup routines
|
||||
#define HEADALT 17 // Lock the current heading and altitude
|
||||
enum mode {
|
||||
MANUAL=0,
|
||||
LEARNING=2,
|
||||
AUTO=10,
|
||||
RTL=11,
|
||||
GUIDED=15,
|
||||
INITIALISING=16
|
||||
};
|
||||
|
||||
// Commands - Note that APM now uses a subset of the MAVLink protocol commands. See enum MAV_CMD in the GCS_Mavlink library
|
||||
#define CMD_BLANK 0 // there is no command stored in the mem location requested
|
||||
|
||||
@ -1,33 +1,6 @@
|
||||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
|
||||
static void failsafe_short_on_event(int fstype)
|
||||
{
|
||||
// This is how to handle a short loss of control signal failsafe.
|
||||
failsafe = fstype;
|
||||
ch3_failsafe_timer = millis();
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event on, "));
|
||||
switch(control_mode)
|
||||
{
|
||||
case MANUAL:
|
||||
case LEARNING:
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
case GUIDED:
|
||||
if(g.short_fs_action == 1) {
|
||||
set_mode(RTL);
|
||||
}
|
||||
break;
|
||||
|
||||
case CIRCLE:
|
||||
case RTL:
|
||||
default:
|
||||
break;
|
||||
}
|
||||
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
|
||||
}
|
||||
|
||||
static void failsafe_long_on_event(int fstype)
|
||||
{
|
||||
// This is how to handle a long loss of control signal failsafe.
|
||||
@ -38,7 +11,6 @@ static void failsafe_long_on_event(int fstype)
|
||||
{
|
||||
case MANUAL:
|
||||
case LEARNING:
|
||||
case CIRCLE:
|
||||
set_mode(RTL);
|
||||
break;
|
||||
|
||||
@ -56,21 +28,6 @@ static void failsafe_long_on_event(int fstype)
|
||||
gcs_send_text_fmt(PSTR("flight mode = %u"), (unsigned)control_mode);
|
||||
}
|
||||
|
||||
static void failsafe_short_off_event()
|
||||
{
|
||||
// We're back in radio contact
|
||||
gcs_send_text_P(SEVERITY_LOW, PSTR("Failsafe - Short event off"));
|
||||
failsafe = FAILSAFE_NONE;
|
||||
|
||||
// re-read the switch so we can return to our preferred mode
|
||||
// --------------------------------------------------------
|
||||
reset_control_switch();
|
||||
|
||||
// Reset control integrators
|
||||
// ---------------------
|
||||
reset_I();
|
||||
}
|
||||
|
||||
#if BATTERY_EVENT == ENABLED
|
||||
static void low_battery_event(void)
|
||||
{
|
||||
|
||||
@ -38,14 +38,6 @@ static void navigate()
|
||||
}
|
||||
|
||||
|
||||
static void calc_gndspeed_undershoot()
|
||||
{
|
||||
if (g_gps->status() == GPS::GPS_OK) {
|
||||
// Function is overkill, but here in case we want to add filtering later
|
||||
groundspeed_undershoot = (g.min_gndspeed > 0) ? (g.min_gndspeed - ground_speed) : 0;
|
||||
}
|
||||
}
|
||||
|
||||
static void calc_bearing_error()
|
||||
{
|
||||
bearing_error = nav_bearing - ahrs.yaw_sensor;
|
||||
@ -71,7 +63,7 @@ static void update_crosstrack(void)
|
||||
// Crosstrack Error
|
||||
// ----------------
|
||||
if (abs(wrap_180(target_bearing - crosstrack_bearing)) < 4500) { // If we are too far off or too close we don't do track following
|
||||
crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * (float)wp_distance; // Meters we are off track line
|
||||
crosstrack_error = sinf(radians((target_bearing - crosstrack_bearing) / (float)100)) * wp_distance; // Meters we are off track line
|
||||
nav_bearing += constrain(crosstrack_error * g.crosstrack_gain, -g.crosstrack_entry_angle.get(), g.crosstrack_entry_angle.get());
|
||||
nav_bearing = wrap_360(nav_bearing);
|
||||
}
|
||||
|
||||
@ -8,22 +8,13 @@ static uint8_t failsafeCounter = 0; // we wait a second to take over the thrott
|
||||
static void init_rc_in()
|
||||
{
|
||||
// set rc channel ranges
|
||||
g.channel_roll.set_angle(SERVO_MAX);
|
||||
g.channel_pitch.set_angle(SERVO_MAX);
|
||||
g.channel_rudder.set_angle(SERVO_MAX);
|
||||
g.channel_steer.set_angle(SERVO_MAX);
|
||||
g.channel_throttle.set_angle(100);
|
||||
|
||||
// set rc dead zones
|
||||
g.channel_roll.set_dead_zone(60);
|
||||
g.channel_pitch.set_dead_zone(60);
|
||||
g.channel_rudder.set_dead_zone(60);
|
||||
g.channel_steer.set_dead_zone(60);
|
||||
g.channel_throttle.set_dead_zone(6);
|
||||
|
||||
//g.channel_roll.dead_zone = 60;
|
||||
//g.channel_pitch.dead_zone = 60;
|
||||
//g.channel_rudder.dead_zone = 60;
|
||||
//g.channel_throttle.dead_zone = 6;
|
||||
|
||||
//set auxiliary ranges
|
||||
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
|
||||
}
|
||||
@ -40,10 +31,8 @@ static void init_rc_out()
|
||||
hal.rcout->enable_ch(CH_8);
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
hal.rcout->write(CH_1, g.channel_roll.radio_trim); // Initialization of servo outputs
|
||||
hal.rcout->write(CH_2, g.channel_pitch.radio_trim);
|
||||
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_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);
|
||||
@ -65,11 +54,9 @@ static void init_rc_out()
|
||||
|
||||
static void read_radio()
|
||||
{
|
||||
g.channel_roll.set_pwm(hal.rcin->read(CH_ROLL));
|
||||
g.channel_pitch.set_pwm(hal.rcin->read(CH_PITCH));
|
||||
g.channel_steer.set_pwm(hal.rcin->read(CH_ROLL));
|
||||
|
||||
g.channel_throttle.set_pwm(hal.rcin->read(CH_3));
|
||||
g.channel_rudder.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));
|
||||
@ -144,22 +131,11 @@ static void trim_control_surfaces()
|
||||
read_radio();
|
||||
// Store control surface trim values
|
||||
// ---------------------------------
|
||||
if ((g.channel_roll.radio_in > 1400) && (g.channel_pitch.radio_trim > 1400)) {
|
||||
g.channel_roll.radio_trim = g.channel_roll.radio_max + g.channel_roll.radio_min - g.channel_roll.radio_in;
|
||||
g.channel_pitch.radio_trim = g.channel_pitch.radio_max + g.channel_pitch.radio_min - g.channel_pitch.radio_in;
|
||||
g.channel_rudder.radio_trim = g.channel_rudder.radio_max + g.channel_rudder.radio_min - g.channel_rudder.radio_in;
|
||||
RC_Channel_aux::set_radio_trim(RC_Channel_aux::k_aileron);
|
||||
} else {
|
||||
g.channel_roll.radio_trim = 1500; // case of HIL test without receiver active
|
||||
g.channel_pitch.radio_trim = 1500;
|
||||
g.channel_rudder.radio_trim = 1500;
|
||||
g.channel_throttle.radio_trim = 1000;
|
||||
if (g.channel_steer.radio_in > 1400) {
|
||||
g.channel_steer.radio_trim = g.channel_steer.radio_in;
|
||||
// save to eeprom
|
||||
g.channel_steer.save_eeprom();
|
||||
}
|
||||
|
||||
// save to eeprom
|
||||
g.channel_roll.save_eeprom();
|
||||
g.channel_pitch.save_eeprom();
|
||||
g.channel_rudder.save_eeprom();
|
||||
}
|
||||
|
||||
static void trim_radio()
|
||||
|
||||
@ -65,7 +65,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
|
||||
report_gains();
|
||||
report_xtrack();
|
||||
report_throttle();
|
||||
report_flight_modes();
|
||||
report_modes();
|
||||
report_compass();
|
||||
|
||||
cliSerial->printf_P(PSTR("Raw Values\n"));
|
||||
@ -94,8 +94,6 @@ setup_factory(uint8_t argc, const Menu::arg *argv)
|
||||
AP_Param::erase_all();
|
||||
cliSerial->printf_P(PSTR("\nFACTORY RESET complete - please reset APM to continue"));
|
||||
|
||||
//default_flight_modes(); // This will not work here. Replacement code located in init_ardupilot()
|
||||
|
||||
for (;;) {
|
||||
}
|
||||
// note, cannot actually return here
|
||||
@ -117,7 +115,7 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
|
||||
if(g.channel_roll.radio_in < 500){
|
||||
if(g.channel_steer.radio_in < 500){
|
||||
while(1){
|
||||
cliSerial->printf_P(PSTR("\nNo radio; Check connectors."));
|
||||
delay(1000);
|
||||
@ -125,27 +123,27 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
}
|
||||
|
||||
g.channel_roll.radio_min = g.channel_roll.radio_in;
|
||||
g.channel_pitch.radio_min = g.channel_pitch.radio_in;
|
||||
g.channel_steer.radio_min = g.channel_steer.radio_in;
|
||||
g.channel_throttle.radio_min = g.channel_throttle.radio_in;
|
||||
g.channel_rudder.radio_min = g.channel_rudder.radio_in;
|
||||
g.rc_2.radio_min = g.rc_2.radio_in;
|
||||
g.rc_4.radio_min = g.rc_4.radio_in;
|
||||
g.rc_5.radio_min = g.rc_5.radio_in;
|
||||
g.rc_6.radio_min = g.rc_6.radio_in;
|
||||
g.rc_7.radio_min = g.rc_7.radio_in;
|
||||
g.rc_8.radio_min = g.rc_8.radio_in;
|
||||
|
||||
g.channel_roll.radio_max = g.channel_roll.radio_in;
|
||||
g.channel_pitch.radio_max = g.channel_pitch.radio_in;
|
||||
g.channel_steer.radio_max = g.channel_steer.radio_in;
|
||||
g.channel_throttle.radio_max = g.channel_throttle.radio_in;
|
||||
g.channel_rudder.radio_max = g.channel_rudder.radio_in;
|
||||
g.rc_2.radio_max = g.rc_2.radio_in;
|
||||
g.rc_4.radio_max = g.rc_4.radio_in;
|
||||
g.rc_5.radio_max = g.rc_5.radio_in;
|
||||
g.rc_6.radio_max = g.rc_6.radio_in;
|
||||
g.rc_7.radio_max = g.rc_7.radio_in;
|
||||
g.rc_8.radio_max = g.rc_8.radio_in;
|
||||
|
||||
g.channel_roll.radio_trim = g.channel_roll.radio_in;
|
||||
g.channel_pitch.radio_trim = g.channel_pitch.radio_in;
|
||||
g.channel_rudder.radio_trim = g.channel_rudder.radio_in;
|
||||
g.channel_steer.radio_trim = g.channel_steer.radio_in;
|
||||
g.rc_2.radio_trim = 1500;
|
||||
g.rc_4.radio_trim = 1500;
|
||||
g.rc_5.radio_trim = 1500;
|
||||
g.rc_6.radio_trim = 1500;
|
||||
g.rc_7.radio_trim = 1500;
|
||||
@ -159,10 +157,10 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
// ----------------------------------------------------------
|
||||
read_radio();
|
||||
|
||||
g.channel_roll.update_min_max();
|
||||
g.channel_pitch.update_min_max();
|
||||
g.channel_steer.update_min_max();
|
||||
g.channel_throttle.update_min_max();
|
||||
g.channel_rudder.update_min_max();
|
||||
g.rc_2.update_min_max();
|
||||
g.rc_4.update_min_max();
|
||||
g.rc_5.update_min_max();
|
||||
g.rc_6.update_min_max();
|
||||
g.rc_7.update_min_max();
|
||||
@ -172,10 +170,10 @@ setup_radio(uint8_t argc, const Menu::arg *argv)
|
||||
while (cliSerial->available() > 0) {
|
||||
cliSerial->read();
|
||||
}
|
||||
g.channel_roll.save_eeprom();
|
||||
g.channel_pitch.save_eeprom();
|
||||
g.channel_steer.save_eeprom();
|
||||
g.channel_throttle.save_eeprom();
|
||||
g.channel_rudder.save_eeprom();
|
||||
g.rc_2.save_eeprom();
|
||||
g.rc_4.save_eeprom();
|
||||
g.rc_5.save_eeprom();
|
||||
g.rc_6.save_eeprom();
|
||||
g.rc_7.save_eeprom();
|
||||
@ -209,10 +207,10 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
if (oldSwitchPosition != switchPosition){
|
||||
// force position 5 to MANUAL
|
||||
if (switchPosition > 4) {
|
||||
flight_modes[switchPosition] = MANUAL;
|
||||
modes[switchPosition] = MANUAL;
|
||||
}
|
||||
// update our current mode
|
||||
mode = flight_modes[switchPosition];
|
||||
mode = modes[switchPosition];
|
||||
|
||||
// update the user
|
||||
print_switch(switchPosition, mode);
|
||||
@ -230,7 +228,6 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
while (
|
||||
mode != MANUAL &&
|
||||
mode != CIRCLE &&
|
||||
mode != LEARNING &&
|
||||
mode != AUTO &&
|
||||
mode != RTL)
|
||||
@ -248,7 +245,7 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
mode = MANUAL;
|
||||
|
||||
// save new mode
|
||||
flight_modes[switchPosition] = mode;
|
||||
modes[switchPosition] = mode;
|
||||
|
||||
// print new mode
|
||||
print_switch(switchPosition, mode);
|
||||
@ -258,8 +255,8 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
if(cliSerial->available() > 0){
|
||||
// save changes
|
||||
for (mode=0; mode<6; mode++)
|
||||
flight_modes[mode].save();
|
||||
report_flight_modes();
|
||||
modes[mode].save();
|
||||
report_modes();
|
||||
print_done();
|
||||
return (0);
|
||||
}
|
||||
@ -388,26 +385,14 @@ static void report_gains()
|
||||
cliSerial->printf_P(PSTR("Gains\n"));
|
||||
print_divider();
|
||||
|
||||
cliSerial->printf_P(PSTR("servo roll:\n"));
|
||||
print_PID(&g.pidServoRoll);
|
||||
cliSerial->printf_P(PSTR("servo steer:\n"));
|
||||
print_PID(&g.pidServoSteer);
|
||||
|
||||
cliSerial->printf_P(PSTR("servo pitch:\n"));
|
||||
print_PID(&g.pidServoPitch);
|
||||
cliSerial->printf_P(PSTR("nav steer:\n"));
|
||||
print_PID(&g.pidNavSteer);
|
||||
|
||||
cliSerial->printf_P(PSTR("servo rudder:\n"));
|
||||
print_PID(&g.pidServoRudder);
|
||||
|
||||
cliSerial->printf_P(PSTR("nav roll:\n"));
|
||||
print_PID(&g.pidNavRoll);
|
||||
|
||||
cliSerial->printf_P(PSTR("nav pitch airspeed:\n"));
|
||||
print_PID(&g.pidNavPitchAirspeed);
|
||||
|
||||
cliSerial->printf_P(PSTR("energry throttle:\n"));
|
||||
print_PID(&g.pidTeThrottle);
|
||||
|
||||
cliSerial->printf_P(PSTR("nav pitch alt:\n"));
|
||||
print_PID(&g.pidNavPitchAltitude);
|
||||
cliSerial->printf_P(PSTR("speed throttle:\n"));
|
||||
print_PID(&g.pidSpeedThrottle);
|
||||
|
||||
print_blanks(2);
|
||||
}
|
||||
@ -482,14 +467,14 @@ static void report_compass()
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
static void report_flight_modes()
|
||||
static void report_modes()
|
||||
{
|
||||
//print_blanks(2);
|
||||
cliSerial->printf_P(PSTR("Flight modes\n"));
|
||||
print_divider();
|
||||
|
||||
for(int i = 0; i < 6; i++ ){
|
||||
print_switch(i, flight_modes[i]);
|
||||
print_switch(i, modes[i]);
|
||||
}
|
||||
print_blanks(2);
|
||||
}
|
||||
@ -511,10 +496,10 @@ print_PID(PID * pid)
|
||||
static void
|
||||
print_radio_values()
|
||||
{
|
||||
cliSerial->printf_P(PSTR("CH1: %d | %d | %d\n"), (int)g.channel_roll.radio_min, (int)g.channel_roll.radio_trim, (int)g.channel_roll.radio_max);
|
||||
cliSerial->printf_P(PSTR("CH2: %d | %d | %d\n"), (int)g.channel_pitch.radio_min, (int)g.channel_pitch.radio_trim, (int)g.channel_pitch.radio_max);
|
||||
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("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("CH4: %d | %d | %d\n"), (int)g.channel_rudder.radio_min, (int)g.channel_rudder.radio_trim, (int)g.channel_rudder.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("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("CH7: %d | %d | %d\n"), (int)g.rc_7.radio_min, (int)g.rc_7.radio_trim, (int)g.rc_7.radio_max);
|
||||
@ -526,7 +511,7 @@ static void
|
||||
print_switch(uint8_t p, uint8_t m)
|
||||
{
|
||||
cliSerial->printf_P(PSTR("Pos %d: "),p);
|
||||
print_flight_mode(m);
|
||||
print_mode(m);
|
||||
}
|
||||
|
||||
static void
|
||||
@ -559,10 +544,10 @@ radio_input_switch(void)
|
||||
static int8_t bouncer = 0;
|
||||
|
||||
|
||||
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) > 100) {
|
||||
if (int16_t(g.channel_steer.radio_in - g.channel_steer.radio_trim) > 100) {
|
||||
bouncer = 10;
|
||||
}
|
||||
if (int16_t(g.channel_roll.radio_in - g.channel_roll.radio_trim) < -100) {
|
||||
if (int16_t(g.channel_steer.radio_in - g.channel_steer.radio_trim) < -100) {
|
||||
bouncer = -10;
|
||||
}
|
||||
if (bouncer >0) {
|
||||
|
||||
@ -334,16 +334,13 @@ static void startup_ground(void)
|
||||
gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
|
||||
}
|
||||
|
||||
static void set_mode(uint8_t mode)
|
||||
static void set_mode(enum mode mode)
|
||||
{
|
||||
|
||||
if(control_mode == mode){
|
||||
// don't switch modes if we are already in the correct mode.
|
||||
return;
|
||||
}
|
||||
if(g.auto_trim > 0 && control_mode == MANUAL)
|
||||
trim_control_surfaces();
|
||||
|
||||
control_mode = mode;
|
||||
throttle_last = 0;
|
||||
throttle = 500;
|
||||
@ -352,7 +349,6 @@ static void set_mode(uint8_t mode)
|
||||
{
|
||||
case MANUAL:
|
||||
case LEARNING:
|
||||
case CIRCLE:
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
@ -398,23 +394,6 @@ static void check_long_failsafe()
|
||||
}
|
||||
}
|
||||
|
||||
static void check_short_failsafe()
|
||||
{
|
||||
// only act on changes
|
||||
// -------------------
|
||||
if(failsafe == FAILSAFE_NONE){
|
||||
if(ch3_failsafe) { // The condition is checked and the flag ch3_failsafe is set in radio.pde
|
||||
failsafe_short_on_event(FAILSAFE_SHORT);
|
||||
}
|
||||
}
|
||||
|
||||
if(failsafe == FAILSAFE_SHORT){
|
||||
if(!ch3_failsafe) {
|
||||
failsafe_short_off_event();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if LITE == DISABLED
|
||||
static void startup_INS_ground(bool force_accel_level)
|
||||
{
|
||||
@ -547,7 +526,7 @@ uint16_t board_voltage(void)
|
||||
}
|
||||
|
||||
static void
|
||||
print_flight_mode(uint8_t mode)
|
||||
print_mode(uint8_t mode)
|
||||
{
|
||||
switch (mode) {
|
||||
case MANUAL:
|
||||
|
||||
@ -90,10 +90,10 @@ test_radio_pwm(uint8_t argc, const Menu::arg *argv)
|
||||
read_radio();
|
||||
|
||||
cliSerial->printf_P(PSTR("IN:\t1: %d\t2: %d\t3: %d\t4: %d\t5: %d\t6: %d\t7: %d\t8: %d\n"),
|
||||
g.channel_roll.radio_in,
|
||||
g.channel_pitch.radio_in,
|
||||
g.channel_steer.radio_in,
|
||||
g.rc_2.radio_in,
|
||||
g.channel_throttle.radio_in,
|
||||
g.channel_rudder.radio_in,
|
||||
g.rc_4.radio_in,
|
||||
g.rc_5.radio_in,
|
||||
g.rc_6.radio_in,
|
||||
g.rc_7.radio_in,
|
||||
@ -146,10 +146,8 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
||||
delay(20);
|
||||
read_radio();
|
||||
|
||||
g.channel_roll.calc_pwm();
|
||||
g.channel_pitch.calc_pwm();
|
||||
g.channel_steer.calc_pwm();
|
||||
g.channel_throttle.calc_pwm();
|
||||
g.channel_rudder.calc_pwm();
|
||||
|
||||
// write out the servo PWM values
|
||||
// ------------------------------
|
||||
@ -158,10 +156,10 @@ test_radio(uint8_t argc, const Menu::arg *argv)
|
||||
tuning_value = constrain(((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"),
|
||||
g.channel_roll.control_in,
|
||||
g.channel_pitch.control_in,
|
||||
g.channel_steer.control_in,
|
||||
g.rc_2.control_in,
|
||||
g.channel_throttle.control_in,
|
||||
g.channel_rudder.control_in,
|
||||
g.rc_4.control_in,
|
||||
g.rc_5.control_in,
|
||||
g.rc_6.control_in,
|
||||
g.rc_7.control_in,
|
||||
@ -207,13 +205,13 @@ test_failsafe(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
if (oldSwitchPosition != readSwitch()){
|
||||
cliSerial->printf_P(PSTR("CONTROL MODE CHANGED: "));
|
||||
print_flight_mode(readSwitch());
|
||||
print_mode(readSwitch());
|
||||
fail_test++;
|
||||
}
|
||||
|
||||
if(g.throttle_fs_enabled && g.channel_throttle.get_failsafe()){
|
||||
cliSerial->printf_P(PSTR("THROTTLE FAILSAFE ACTIVATED: %d, "), g.channel_throttle.radio_in);
|
||||
print_flight_mode(readSwitch());
|
||||
print_mode(readSwitch());
|
||||
fail_test++;
|
||||
}
|
||||
|
||||
@ -293,8 +291,8 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
delay(1000);
|
||||
|
||||
cliSerial->printf_P(PSTR("%d waypoints\n"), (int)g.command_total);
|
||||
cliSerial->printf_P(PSTR("Hit radius: %d\n"), (int)g.waypoint_radius);
|
||||
cliSerial->printf_P(PSTR("%u waypoints\n"), (unsigned)g.command_total);
|
||||
cliSerial->printf_P(PSTR("Hit radius: %f\n"), g.waypoint_radius);
|
||||
|
||||
for(uint8_t i = 0; i <= g.command_total; i++){
|
||||
struct Location temp = get_cmd_with_index(i);
|
||||
@ -325,7 +323,7 @@ test_modeswitch(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
cliSerial->printf_P(PSTR("Control CH "));
|
||||
|
||||
cliSerial->println(FLIGHT_MODE_CHANNEL, DEC);
|
||||
cliSerial->println(MODE_CHANNEL, DEC);
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user