mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-05 23:43:58 -04:00
AC2.0.24b
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2567 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort
parent
c5f06b99fe
commit
cb168c399d
@ -7,13 +7,17 @@
|
||||
//#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD
|
||||
|
||||
#define NAV_TEST 1 // 0 = traditional, 1 = rate controlled
|
||||
#define YAW_OPTION 0 // 0 = hybrid rate approach, 1 = offset Yaw approach
|
||||
#define YAW_OPTION 1 // 0 = hybrid rate approach, 1 = offset Yaw approach, 2 = simple hybrid
|
||||
#define AUTO_RESET_LOITER 1 // enables Loiter to reset it's current location based on stick input.
|
||||
#define CUT_MOTORS 1 // do we cut the motors with no throttle?
|
||||
|
||||
#define DYNAMIC_DRIFT 0 // careful!!! 0 = off, 1 = on
|
||||
|
||||
// do we want to have camera stabilization?
|
||||
#define CAMERA_STABILIZER ENABLED
|
||||
|
||||
#define BROKEN_SLIDER 0
|
||||
|
||||
#define FRAME_CONFIG QUAD_FRAME
|
||||
/*
|
||||
options:
|
||||
@ -73,4 +77,4 @@
|
||||
#define FR_LED AN12 // Mega PE4 pin, OUT7
|
||||
#define RE_LED AN14 // Mega PE5 pin, OUT6
|
||||
#define RI_LED AN10 // Mega PH4 pin, OUT5
|
||||
#define LE_LED AN8 // Mega PH5 pin, OUT4
|
||||
#define LE_LED AN8 // Mega PH5 pin, OUT4
|
||||
|
||||
@ -107,7 +107,7 @@ GPS *g_gps;
|
||||
AP_Compass_HMC5883L compass(Parameters::k_param_compass);
|
||||
#else
|
||||
#error Unrecognised MAG_PROTOCOL setting.
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// real GPS selection
|
||||
#if GPS_PROTOCOL == GPS_PROTOCOL_AUTO
|
||||
@ -191,7 +191,15 @@ GPS *g_gps;
|
||||
//#include <GCS_SIMPLE.h>
|
||||
//GCS_SIMPLE gcs_simple(&Serial);
|
||||
|
||||
AP_RangeFinder_MaxsonarXL sonar;
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// SONAR selection
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
#if SONAR_TYPE == MAX_SONAR_XL
|
||||
AP_RangeFinder_MaxsonarXL sonar;//(SONAR_PORT, &adc);
|
||||
#elif SONAR_TYPE == MAX_SONAR_LV
|
||||
AP_RangeFinder_MaxsonarLV sonar;//(SONAR_PORT, &adc);
|
||||
#endif
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Global variables
|
||||
@ -255,6 +263,7 @@ bool did_clear_yaw_control;
|
||||
boolean motor_light; // status of the Motor safety
|
||||
boolean GPS_light; // status of the GPS light
|
||||
boolean timer_light; // status of the Motor safety
|
||||
byte led_mode = NORMAL_LEDS;
|
||||
|
||||
// GPS variables
|
||||
// -------------
|
||||
@ -321,9 +330,11 @@ float current_total;
|
||||
|
||||
// Barometer Sensor variables
|
||||
// --------------------------
|
||||
unsigned long abs_pressure;
|
||||
unsigned long ground_pressure;
|
||||
long abs_pressure;
|
||||
long ground_pressure;
|
||||
int ground_temperature;
|
||||
int32_t baro_filter[BARO_FILTER_SIZE];
|
||||
byte baro_filter_index;
|
||||
|
||||
// Altitude Sensor variables
|
||||
// ----------------------
|
||||
@ -340,13 +351,14 @@ boolean land_complete;
|
||||
int landing_distance; // meters;
|
||||
long old_alt; // used for managing altitude rates
|
||||
int velocity_land;
|
||||
byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
|
||||
byte yaw_tracking = MAV_ROI_WPNEXT; // no tracking, point at next wp, or at a target
|
||||
byte brake_timer;
|
||||
|
||||
// Loiter management
|
||||
// -----------------
|
||||
long saved_target_bearing; // deg * 100
|
||||
long loiter_time; // millis : when we started LOITER mode
|
||||
long loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
unsigned long loiter_time; // millis : when we started LOITER mode
|
||||
unsigned long loiter_time_max; // millis : how long to stay in LOITER mode
|
||||
|
||||
// these are the values for navigation control functions
|
||||
// ----------------------------------------------------
|
||||
@ -364,14 +376,16 @@ bool invalid_nav; // used to control when we calculate nav_throttle
|
||||
bool set_throttle_cruise_flag = false; // used to track the throttle crouse value
|
||||
|
||||
long command_yaw_start; // what angle were we to begin with
|
||||
long command_yaw_start_time; // when did we start turning
|
||||
int command_yaw_time; // how long we are turning
|
||||
unsigned long command_yaw_start_time; // when did we start turning
|
||||
unsigned int command_yaw_time; // how long we are turning
|
||||
long command_yaw_end; // what angle are we trying to be
|
||||
long command_yaw_delta; // how many degrees will we turn
|
||||
int command_yaw_speed; // how fast to turn
|
||||
byte command_yaw_dir;
|
||||
byte command_yaw_relative;
|
||||
|
||||
int auto_level_counter;
|
||||
|
||||
// Waypoints
|
||||
// ---------
|
||||
long wp_distance; // meters - distance between plane and next waypoint
|
||||
@ -381,8 +395,8 @@ byte next_wp_index; // Current active command index
|
||||
// repeating event control
|
||||
// -----------------------
|
||||
byte event_id; // what to do - see defines
|
||||
long event_timer; // when the event was asked for in ms
|
||||
int event_delay; // how long to delay the next firing of event in millis
|
||||
unsigned long event_timer; // when the event was asked for in ms
|
||||
unsigned int event_delay; // how long to delay the next firing of event in millis
|
||||
int event_repeat; // how many times to fire : 0 = forever, 1 = do once, 2 = do twice
|
||||
int event_value; // per command value, such as PWM for servos
|
||||
int event_undo_value; // the value used to undo commands
|
||||
@ -458,6 +472,7 @@ float load; // % MCU cycles used
|
||||
|
||||
byte counter_one_herz;
|
||||
bool GPS_enabled = false;
|
||||
byte loop_step;
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Top-level logic
|
||||
@ -478,6 +493,9 @@ void loop()
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
mainLoop_count++;
|
||||
|
||||
//if (delta_ms_fast_loop > 6)
|
||||
// Log_Write_Performance();
|
||||
|
||||
/*
|
||||
if(delta_ms_fast_loop > 11){
|
||||
update_timer_light(true);
|
||||
@ -500,6 +518,7 @@ void loop()
|
||||
|
||||
fifty_hz_loop();
|
||||
counter_one_herz++;
|
||||
|
||||
if(counter_one_herz == 50){
|
||||
super_slow_loop();
|
||||
counter_one_herz = 0;
|
||||
@ -519,7 +538,7 @@ void loop()
|
||||
}
|
||||
}
|
||||
|
||||
// Main loop 160Hz
|
||||
// Main loop
|
||||
void fast_loop()
|
||||
{
|
||||
// IMU DCM Algorithm
|
||||
@ -532,8 +551,7 @@ void fast_loop()
|
||||
|
||||
// Read radio
|
||||
// ----------
|
||||
if (APM_RC.GetState() == 1)
|
||||
read_radio(); // read the radio first
|
||||
read_radio(); // read the radio first
|
||||
|
||||
// custom code/exceptions for flight modes
|
||||
// ---------------------------------------
|
||||
@ -566,6 +584,8 @@ void medium_loop()
|
||||
// This case deals with the GPS and Compass
|
||||
//-----------------------------------------
|
||||
case 0:
|
||||
loop_step = 1;
|
||||
|
||||
medium_loopCounter++;
|
||||
|
||||
if(GPS_enabled){
|
||||
@ -582,11 +602,15 @@ void medium_loop()
|
||||
compass.null_offsets(dcm.get_dcm_matrix());
|
||||
}
|
||||
#endif
|
||||
|
||||
// auto_trim, uses an auto_level algorithm
|
||||
auto_trim();
|
||||
break;
|
||||
|
||||
// This case performs some navigation computations
|
||||
//------------------------------------------------
|
||||
case 1:
|
||||
loop_step = 2;
|
||||
medium_loopCounter++;
|
||||
|
||||
// hack to stop navigation in Simple mode
|
||||
@ -597,7 +621,10 @@ void medium_loop()
|
||||
}
|
||||
|
||||
// Auto control modes:
|
||||
if(g_gps->new_data){
|
||||
if(g_gps->new_data && g_gps->fix){
|
||||
loop_step = 11;
|
||||
|
||||
// invalidate GPS data
|
||||
g_gps->new_data = false;
|
||||
|
||||
// we are not tracking I term on navigation, so this isn't needed
|
||||
@ -621,6 +648,7 @@ void medium_loop()
|
||||
// command processing
|
||||
//-------------------
|
||||
case 2:
|
||||
loop_step = 3;
|
||||
medium_loopCounter++;
|
||||
|
||||
// Read altitude from sensors
|
||||
@ -636,6 +664,13 @@ void medium_loop()
|
||||
// invalidate the throttle hold value
|
||||
// ----------------------------------
|
||||
invalid_throttle = true;
|
||||
break;
|
||||
|
||||
// This case deals with sending high rate telemetry
|
||||
//-------------------------------------------------
|
||||
case 3:
|
||||
loop_step = 4;
|
||||
medium_loopCounter++;
|
||||
|
||||
// perform next command
|
||||
// --------------------
|
||||
@ -644,12 +679,6 @@ void medium_loop()
|
||||
update_commands();
|
||||
//}
|
||||
}
|
||||
break;
|
||||
|
||||
// This case deals with sending high rate telemetry
|
||||
//-------------------------------------------------
|
||||
case 3:
|
||||
medium_loopCounter++;
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED)
|
||||
@ -672,11 +701,17 @@ void medium_loop()
|
||||
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
|
||||
hil.data_stream_send(5,45);
|
||||
#endif
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_MOTORS)
|
||||
Log_Write_Motors();
|
||||
|
||||
|
||||
break;
|
||||
|
||||
// This case controls the slow loop
|
||||
//---------------------------------
|
||||
case 4:
|
||||
loop_step = 5;
|
||||
medium_loopCounter = 0;
|
||||
|
||||
delta_ms_medium_loop = millis() - medium_loopTimer;
|
||||
@ -695,6 +730,10 @@ void medium_loop()
|
||||
// -----------------------
|
||||
arm_motors();
|
||||
|
||||
// should we be in DCM Fast recovery?
|
||||
// ----------------------------------
|
||||
check_DCM();
|
||||
|
||||
slow_loop();
|
||||
break;
|
||||
|
||||
@ -756,9 +795,10 @@ void fifty_hz_loop()
|
||||
// XXX this should be absorbed into the above,
|
||||
// or be a "GCS fast loop" interface
|
||||
|
||||
#if FRAME_CONFIG == TRI_FRAME
|
||||
#if FRAME_CONFIG == TRI_FRAME
|
||||
// Hack - had to move to 50hz loop to test a theory
|
||||
// servo Yaw
|
||||
g.rc_4.calc_pwm();
|
||||
APM_RC.OutputCh(CH_7, g.rc_4.radio_out);
|
||||
#endif
|
||||
}
|
||||
@ -770,6 +810,7 @@ void slow_loop()
|
||||
//----------------------------------------
|
||||
switch (slow_loopCounter){
|
||||
case 0:
|
||||
loop_step = 6;
|
||||
slow_loopCounter++;
|
||||
superslow_loopCounter++;
|
||||
|
||||
@ -784,6 +825,7 @@ void slow_loop()
|
||||
break;
|
||||
|
||||
case 1:
|
||||
loop_step = 7;
|
||||
slow_loopCounter++;
|
||||
|
||||
// Read 3-position switch on radio
|
||||
@ -810,11 +852,12 @@ void slow_loop()
|
||||
break;
|
||||
|
||||
case 2:
|
||||
loop_step = 8;
|
||||
slow_loopCounter = 0;
|
||||
update_events();
|
||||
|
||||
// blink if we are armed
|
||||
update_motor_light();
|
||||
update_lights();
|
||||
|
||||
// XXX this should be a "GCS slow loop" interface
|
||||
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
|
||||
@ -836,8 +879,8 @@ void slow_loop()
|
||||
|
||||
|
||||
// filter out the baro offset.
|
||||
if(baro_alt_offset > 0) baro_alt_offset--;
|
||||
if(baro_alt_offset < 0) baro_alt_offset++;
|
||||
//if(baro_alt_offset > 0) baro_alt_offset--;
|
||||
//if(baro_alt_offset < 0) baro_alt_offset++;
|
||||
|
||||
|
||||
#if MOTOR_LEDS == 1
|
||||
@ -856,6 +899,7 @@ void slow_loop()
|
||||
// 1Hz loop
|
||||
void super_slow_loop()
|
||||
{
|
||||
loop_step = 9;
|
||||
if (g.log_bitmask & MASK_LOG_CURRENT)
|
||||
Log_Write_Current();
|
||||
|
||||
@ -883,6 +927,7 @@ void super_slow_loop()
|
||||
|
||||
void update_GPS(void)
|
||||
{
|
||||
loop_step = 10;
|
||||
g_gps->update();
|
||||
update_GPS_light();
|
||||
|
||||
@ -1234,14 +1279,51 @@ void update_trig(void){
|
||||
sin_roll_y = temp.c.y / cos_pitch_x;
|
||||
}
|
||||
|
||||
|
||||
// updated at 10hz
|
||||
void update_alt()
|
||||
{
|
||||
altitude_sensor = BARO;
|
||||
|
||||
#if HIL_MODE == HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE == HIL_MODE_ATTITUDE
|
||||
current_loc.alt = g_gps->altitude;
|
||||
#else
|
||||
#else
|
||||
|
||||
if(g.sonar_enabled){
|
||||
// filter out offset
|
||||
float scale;
|
||||
|
||||
// read barometer
|
||||
baro_alt = read_barometer();
|
||||
int temp_sonar = sonar.read();
|
||||
|
||||
// spike filter
|
||||
//if((temp_sonar - sonar_alt) < 50){
|
||||
sonar_alt = temp_sonar;
|
||||
//}
|
||||
|
||||
scale = (sonar_alt - 300) / 300;
|
||||
scale = constrain(scale, 0, 1);
|
||||
|
||||
current_loc.alt = ((float)sonar_alt * (1.0 - scale)) + ((float)baro_alt * scale) + home.alt;
|
||||
|
||||
}else{
|
||||
baro_alt = read_barometer();
|
||||
// no sonar altitude
|
||||
current_loc.alt = baro_alt + home.alt;
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
// updated at 10hz
|
||||
void update_alt2()
|
||||
{
|
||||
altitude_sensor = BARO;
|
||||
|
||||
#if HIL_MODE == HIL_MODE_ATTITUDE
|
||||
current_loc.alt = g_gps->altitude;
|
||||
#else
|
||||
|
||||
if(g.sonar_enabled){
|
||||
// filter out offset
|
||||
|
||||
@ -1254,11 +1336,8 @@ void update_alt()
|
||||
sonar_alt = temp_sonar;
|
||||
}
|
||||
|
||||
//sonar_alt = sonar.read();
|
||||
|
||||
// decide if we're using sonar
|
||||
if ((baro_alt < 1200) || sonar_alt < 300){
|
||||
//if (baro_alt < 700){
|
||||
// correct alt for angle of the sonar
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
temp = max(temp, 0.707);
|
||||
@ -1285,7 +1364,7 @@ void update_alt()
|
||||
}
|
||||
|
||||
//Serial.printf("b_alt: %ld, home: %ld ", baro_alt, home.alt);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
@ -1337,19 +1416,25 @@ void tuning(){
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_PMAX
|
||||
g.pitch_max.set(g.rc_6.control_in * 2); // 0 to 2000
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_DCM_RP
|
||||
dcm.kp_roll_pitch((float)g.rc_6.control_in / 5000.0);
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_DCM_Y
|
||||
dcm.kp_yaw((float)g.rc_6.control_in / 1000.0);
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_KP
|
||||
// yaw heading
|
||||
g.pid_yaw.kP((float)g.rc_6.control_in / 200.0); // range from 0.0 ~ 5.0
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_KD
|
||||
// yaw heading
|
||||
g.pid_yaw.kD((float)g.rc_6.control_in / 1000.0);
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
|
||||
// yaw rate
|
||||
g.pid_acro_rate_yaw.kP((float)g.rc_6.control_in / 1000.0);
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD
|
||||
// yaw rate
|
||||
g.pid_acro_rate_yaw.kD((float)g.rc_6.control_in / 1000.0);
|
||||
@ -1388,4 +1473,21 @@ void update_nav_yaw()
|
||||
}else if(yaw_tracking == MAV_ROI_WPNEXT){
|
||||
nav_yaw = target_bearing;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void check_DCM()
|
||||
{
|
||||
#if DYNAMIC_DRIFT == 1
|
||||
if(g.rc_1.control_in == 0 && g.rc_2.control_in == 0){
|
||||
if(g.rc_3.control_in < (g.throttle_cruise + 20)){
|
||||
//dcm.kp_roll_pitch(dcm.kDCM_kp_rp_high);
|
||||
dcm.kp_roll_pitch(0.15);
|
||||
}
|
||||
}else{
|
||||
dcm.kp_roll_pitch(0.05967);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -30,8 +30,7 @@ limit_nav_pitch_roll(long pmax)
|
||||
void
|
||||
output_stabilize_roll()
|
||||
{
|
||||
float error;//, rate;
|
||||
//int dampener;
|
||||
float error;
|
||||
|
||||
error = g.rc_1.servo_out - dcm.roll_sensor;
|
||||
|
||||
@ -52,8 +51,7 @@ output_stabilize_roll()
|
||||
void
|
||||
output_stabilize_pitch()
|
||||
{
|
||||
float error, rate;
|
||||
int dampener;
|
||||
float error;
|
||||
|
||||
error = g.rc_2.servo_out - dcm.pitch_sensor;
|
||||
|
||||
@ -113,6 +111,7 @@ throttle control
|
||||
void output_manual_throttle()
|
||||
{
|
||||
g.rc_3.servo_out = (float)g.rc_3.control_in * angle_boost();
|
||||
g.rc_3.servo_out = max(g.rc_3.servo_out, 0);
|
||||
}
|
||||
|
||||
// Autopilot
|
||||
@ -125,6 +124,25 @@ void output_auto_throttle()
|
||||
}
|
||||
|
||||
void calc_nav_throttle()
|
||||
{
|
||||
// limit error
|
||||
nav_throttle = g.pid_baro_throttle.get_pid(altitude_error, delta_ms_medium_loop, 1.0);
|
||||
nav_throttle = g.throttle_cruise + constrain(nav_throttle, -60, 60);
|
||||
|
||||
// simple filtering
|
||||
if(nav_throttle_old == 0)
|
||||
nav_throttle_old = nav_throttle;
|
||||
|
||||
nav_throttle = (nav_throttle + nav_throttle_old) >> 1;
|
||||
nav_throttle_old = nav_throttle;
|
||||
|
||||
// clear the new data flag
|
||||
invalid_throttle = false;
|
||||
|
||||
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
|
||||
}
|
||||
|
||||
void calc_nav_throttle2()
|
||||
{
|
||||
// limit error
|
||||
long error = constrain(altitude_error, -400, 400);
|
||||
@ -155,7 +173,6 @@ void calc_nav_throttle()
|
||||
|
||||
//Serial.printf("nav_thr %d, scaler %2.2f ", nav_throttle, scaler);
|
||||
}
|
||||
|
||||
float angle_boost()
|
||||
{
|
||||
float temp = cos_pitch_x * cos_roll_x;
|
||||
@ -205,7 +222,7 @@ output_yaw_with_hold(boolean hold)
|
||||
// look to see if we have exited rate control properly - ie stopped turning
|
||||
if(rate_yaw_flag){
|
||||
// we are still in motion from rate control
|
||||
if(fabs(omega.z) < .2){
|
||||
if((fabs(omega.z) < .25) || (brake_timer < 2)){
|
||||
clear_yaw_control();
|
||||
hold = true; // just to be explicit
|
||||
}else{
|
||||
@ -223,12 +240,15 @@ output_yaw_with_hold(boolean hold)
|
||||
}
|
||||
|
||||
if(hold){
|
||||
brake_timer = 0;
|
||||
// try and hold the current nav_yaw setting
|
||||
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
|
||||
|
||||
// we need to wrap our value so we can be -180 to 180 (*100)
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
yaw_error = constrain(yaw_error, -4000, 4000); // limit error to 40 degees
|
||||
yaw_error = constrain(yaw_error, -9000, 9000); // limit error to 40 degees
|
||||
|
||||
// Apply PID and save the new angle back to RC_Channel
|
||||
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
|
||||
@ -241,25 +261,23 @@ output_yaw_with_hold(boolean hold)
|
||||
}else{
|
||||
|
||||
if(g.rc_4.control_in == 0){
|
||||
|
||||
brake_timer--;
|
||||
// adaptive braking
|
||||
g.rc_4.servo_out = (int)(-1000.0 * omega.z);
|
||||
g.rc_4.servo_out = (int)(-1200.0 * omega.z);
|
||||
|
||||
yaw_debug = YAW_BRAKE; // 1
|
||||
|
||||
}else{
|
||||
// RATE control
|
||||
// Hein, 5/21/11
|
||||
long error = ((long)g.rc_4.control_in * 6) - (rate * 2); // control is += 6000 * 6 = 36000
|
||||
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
|
||||
brake_timer = 100;
|
||||
yaw_debug = YAW_RATE; // 2
|
||||
|
||||
//nav_yaw = dcm.yaw_sensor; // I think this caused the free rotation, dont know why.
|
||||
long error = ((long)g.rc_4.control_in * 6) - (degrees(omega.z) * 100); // control is += 4500 * 6 = 36000
|
||||
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
|
||||
}
|
||||
}
|
||||
|
||||
// Limit Output
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit to 24°
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -3200, 3200); // limit to 32°
|
||||
|
||||
//Serial.printf("%d\n",g.rc_4.servo_out);
|
||||
}
|
||||
@ -271,7 +289,8 @@ output_yaw_with_hold(boolean hold)
|
||||
// re-define nav_yaw if we have stick input
|
||||
if(g.rc_4.control_in != 0){
|
||||
// set nav_yaw + or - the current location
|
||||
nav_yaw = (long)g.rc_4.control_in + dcm.yaw_sensor;
|
||||
//nav_yaw = (long)g.rc_4.control_in + dcm.yaw_sensor;
|
||||
nav_yaw += (long)(g.rc_4.control_in / 90);
|
||||
}
|
||||
|
||||
// we need to wrap our value so we can be 0 to 360 (*100)
|
||||
@ -290,7 +309,40 @@ output_yaw_with_hold(boolean hold)
|
||||
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
|
||||
|
||||
// add in yaw dampener
|
||||
g.rc_4.servo_out -= degrees(omega.z) * 100 * g.pid_yaw.kD();
|
||||
g.rc_4.servo_out -= (degrees(omega.z) * 100) * g.pid_yaw.kD();
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit error to 60 degees
|
||||
}
|
||||
|
||||
#elif YAW_OPTION == 2
|
||||
|
||||
void
|
||||
output_yaw_with_hold(boolean hold)
|
||||
{
|
||||
if(hold){
|
||||
// try and hold the current nav_yaw setting
|
||||
yaw_error = nav_yaw - dcm.yaw_sensor; // +- 60°
|
||||
|
||||
// we need to wrap our value so we can be -180 to 180 (*100)
|
||||
yaw_error = wrap_180(yaw_error);
|
||||
|
||||
// limit the error we're feeding to the PID
|
||||
yaw_error = constrain(yaw_error, -3500, 3500); // limit error to 40 degees
|
||||
|
||||
// Apply PID and save the new angle back to RC_Channel
|
||||
g.rc_4.servo_out = g.pid_yaw.get_pi(yaw_error, delta_ms_fast_loop, 1.0); // .4 * 4000 = 1600
|
||||
|
||||
// add in yaw dampener
|
||||
g.rc_4.servo_out -= (degrees(omega.z) * 100) * g.pid_yaw.kD();
|
||||
|
||||
}else{
|
||||
// RATE control
|
||||
long error = ((long)g.rc_4.control_in * 6) - (degrees(omega.z) * 100); // control is += 4500 * 6 = 36000
|
||||
g.rc_4.servo_out = g.pid_acro_rate_yaw.get_pid(error, delta_ms_fast_loop, 1.0); // kP .07 * 36000 = 2520
|
||||
nav_yaw = dcm.yaw_sensor; // save our Yaw
|
||||
}
|
||||
|
||||
// Limit Output
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -2500, 2500); // limit to 24°
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@ -3,9 +3,9 @@
|
||||
// Code to Write and Read packets from DataFlash log memory
|
||||
// Code to interact with the user to dump or erase logs
|
||||
|
||||
#define HEAD_BYTE1 0xA3 // Decimal 163
|
||||
#define HEAD_BYTE2 0x95 // Decimal 149
|
||||
#define END_BYTE 0xBA // Decimal 186
|
||||
#define HEAD_BYTE1 0xA3 // Decimal 163
|
||||
#define HEAD_BYTE2 0x95 // Decimal 149
|
||||
#define END_BYTE 0xBA // Decimal 186
|
||||
|
||||
|
||||
// These are function definitions so the Menu can be constructed before the functions
|
||||
@ -65,6 +65,7 @@ print_log_menu(void)
|
||||
if (g.log_bitmask & MASK_LOG_RAW) Serial.printf_P(PSTR(" RAW"));
|
||||
if (g.log_bitmask & MASK_LOG_CMD) Serial.printf_P(PSTR(" CMD"));
|
||||
if (g.log_bitmask & MASK_LOG_CURRENT) Serial.printf_P(PSTR(" CURRENT"));
|
||||
if (g.log_bitmask & MASK_LOG_MOTORS) Serial.printf_P(PSTR(" MOTORS"));
|
||||
}
|
||||
|
||||
Serial.println();
|
||||
@ -101,11 +102,10 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
||||
Log_Read(1, 4096);
|
||||
}
|
||||
|
||||
if (/*(argc != 2) || */(dump_log < 1) || (dump_log > last_log_num)) {
|
||||
if (/*(argc != 2) || */ (dump_log < 1) || (dump_log > last_log_num)) {
|
||||
Serial.printf_P(PSTR("bad log # %d\n"), dump_log);
|
||||
Log_Read(1, 4095);
|
||||
return(-1);
|
||||
//Log_Read(1, 4095);
|
||||
return (0);
|
||||
}
|
||||
|
||||
get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end);
|
||||
@ -174,6 +174,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
|
||||
TARG(RAW);
|
||||
TARG(CMD);
|
||||
TARG(CURRENT);
|
||||
TARG(MOTORS);
|
||||
#undef TARG
|
||||
}
|
||||
|
||||
@ -191,6 +192,7 @@ int8_t
|
||||
process_logs(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
log_menu.run();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -453,6 +455,30 @@ void Log_Read_Current()
|
||||
DataFlash.ReadInt());
|
||||
}
|
||||
|
||||
void Log_Write_Motors()
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_MOTORS_MSG);
|
||||
|
||||
DataFlash.WriteInt(motor_out[CH_1]);
|
||||
DataFlash.WriteInt(motor_out[CH_2]);
|
||||
DataFlash.WriteInt(motor_out[CH_3]);
|
||||
DataFlash.WriteInt(motor_out[CH_4]);
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// Read a Current packet
|
||||
void Log_Read_Motors()
|
||||
{
|
||||
Serial.printf_P(PSTR("MOT: %d, %d, %d, %d\n"),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt(),
|
||||
DataFlash.ReadInt());
|
||||
}
|
||||
|
||||
void Log_Write_Nav_Tuning()
|
||||
{
|
||||
Matrix3f tempmat = dcm.get_dcm_matrix();
|
||||
@ -512,13 +538,13 @@ void Log_Write_Control_Tuning()
|
||||
DataFlash.WriteInt((int)(omega.z * 100));
|
||||
|
||||
// Alt hold
|
||||
DataFlash.WriteInt((int)(g.rc_3.servo_out));
|
||||
DataFlash.WriteInt((int)sonar_alt); //
|
||||
DataFlash.WriteInt((int)baro_alt); //
|
||||
DataFlash.WriteInt(g.rc_3.servo_out);
|
||||
DataFlash.WriteInt(sonar_alt); //
|
||||
DataFlash.WriteInt(baro_alt); //
|
||||
|
||||
DataFlash.WriteInt((int)next_WP.alt); //
|
||||
DataFlash.WriteInt((int)altitude_error); //
|
||||
DataFlash.WriteInt((int)g.pid_sonar_throttle.get_integrator());
|
||||
DataFlash.WriteInt((int)g.pid_baro_throttle.get_integrator());
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
@ -559,6 +585,11 @@ void Log_Write_Performance()
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_PERFORMANCE_MSG);
|
||||
|
||||
//DataFlash.WriteByte( delta_ms_fast_loop);
|
||||
//DataFlash.WriteByte( loop_step);
|
||||
|
||||
|
||||
//*
|
||||
DataFlash.WriteLong( millis()- perf_mon_timer);
|
||||
DataFlash.WriteInt( mainLoop_count);
|
||||
DataFlash.WriteInt( G_Dt_max);
|
||||
@ -568,6 +599,7 @@ void Log_Write_Performance()
|
||||
DataFlash.WriteByte( dcm.renorm_blowup_count);
|
||||
DataFlash.WriteByte( gps_fix_count);
|
||||
DataFlash.WriteInt( (int)(dcm.get_health() * 1000));
|
||||
//*/
|
||||
//PM, 20005, 3742, 10,0,0,0,0,89,1000,
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
@ -576,6 +608,7 @@ void Log_Write_Performance()
|
||||
// Read a performance packet
|
||||
void Log_Read_Performance()
|
||||
{
|
||||
//*
|
||||
long pm_time;
|
||||
int logvar;
|
||||
|
||||
@ -594,6 +627,13 @@ void Log_Read_Performance()
|
||||
Serial.print(comma);
|
||||
}
|
||||
Serial.println(" ");
|
||||
//*/
|
||||
|
||||
/*
|
||||
Serial.printf_P(PSTR("PM, %d, %d\n"),
|
||||
DataFlash.ReadByte(),
|
||||
DataFlash.ReadByte());
|
||||
//*/
|
||||
}
|
||||
|
||||
// Write a command processing packet.
|
||||
@ -668,7 +708,7 @@ void Log_Write_Mode(byte mode)
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_MODE_MSG);
|
||||
DataFlash.WriteByte(mode);
|
||||
DataFlash.WriteInt((int)g.throttle_cruise);
|
||||
DataFlash.WriteInt(g.throttle_cruise);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
@ -693,6 +733,22 @@ void Log_Read_Raw()
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
|
||||
void Log_Write_Startup()
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_STARTUP_MSG);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
// Read a mode packet
|
||||
void Log_Read_Startup()
|
||||
{
|
||||
Serial.printf_P(PSTR("START UP\n"));
|
||||
}
|
||||
|
||||
|
||||
// Read the DataFlash log memory : Packet Parser
|
||||
void Log_Read(int start_page, int end_page)
|
||||
{
|
||||
@ -722,64 +778,56 @@ void Log_Read(int start_page, int end_page)
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if(data == LOG_ATTITUDE_MSG){
|
||||
Log_Read_Attitude();
|
||||
log_step++;
|
||||
log_step = 0;
|
||||
switch(data){
|
||||
case LOG_ATTITUDE_MSG:
|
||||
Log_Read_Attitude();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_MODE_MSG){
|
||||
Log_Read_Mode();
|
||||
log_step++;
|
||||
case LOG_MODE_MSG:
|
||||
Log_Read_Mode();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_CONTROL_TUNING_MSG){
|
||||
Log_Read_Control_Tuning();
|
||||
log_step++;
|
||||
case LOG_CONTROL_TUNING_MSG:
|
||||
Log_Read_Control_Tuning();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_NAV_TUNING_MSG){
|
||||
Log_Read_Nav_Tuning();
|
||||
log_step++;
|
||||
case LOG_NAV_TUNING_MSG:
|
||||
Log_Read_Nav_Tuning();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_PERFORMANCE_MSG){
|
||||
Log_Read_Performance();
|
||||
log_step++;
|
||||
case LOG_PERFORMANCE_MSG:
|
||||
Log_Read_Performance();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_RAW_MSG){
|
||||
Log_Read_Raw();
|
||||
log_step++;
|
||||
case LOG_RAW_MSG:
|
||||
Log_Read_Raw();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_CMD_MSG){
|
||||
Log_Read_Cmd();
|
||||
log_step++;
|
||||
case LOG_CMD_MSG:
|
||||
Log_Read_Cmd();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_CURRENT_MSG){
|
||||
Log_Read_Current();
|
||||
log_step++;
|
||||
case LOG_CURRENT_MSG:
|
||||
Log_Read_Current();
|
||||
break;
|
||||
|
||||
}else if(data == LOG_STARTUP_MSG){
|
||||
//Log_Read_Startup();
|
||||
log_step++;
|
||||
}else {
|
||||
if(data == LOG_GPS_MSG){
|
||||
case LOG_STARTUP_MSG:
|
||||
Log_Read_Startup();
|
||||
break;
|
||||
|
||||
case LOG_MOTORS_MSG:
|
||||
Log_Read_Motors();
|
||||
break;
|
||||
|
||||
case LOG_GPS_MSG:
|
||||
Log_Read_GPS();
|
||||
log_step++;
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count);
|
||||
log_step = 0; // Restart, we have a problem...
|
||||
}
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if(data == END_BYTE){
|
||||
packet_count++;
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Error EB: %d\n"),data);
|
||||
}
|
||||
log_step = 0; // Restart sequence: new packet...
|
||||
break;
|
||||
}
|
||||
|
||||
page = DataFlash.GetPage();
|
||||
}
|
||||
//Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -221,7 +221,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
|
||||
(float)airspeed / 100.0,
|
||||
(float)g_gps->ground_speed / 100.0,
|
||||
(dcm.yaw_sensor / 100) % 360,
|
||||
(int)g.rc_3.servo_out,
|
||||
g.rc_3.servo_out,
|
||||
current_loc.alt / 100.0,
|
||||
climb_rate);
|
||||
break;
|
||||
|
||||
@ -17,7 +17,14 @@ public:
|
||||
// The increment will prevent old parameters from being used incorrectly
|
||||
// by newer code.
|
||||
//
|
||||
static const uint16_t k_format_version = 9;
|
||||
static const uint16_t k_format_version = 101;
|
||||
|
||||
// The parameter software_type is set up solely for ground station use
|
||||
// and identifies the software type (eg ArduPilotMega versus ArduCopterMega)
|
||||
// GCS will interpret values 0-9 as ArduPilotMega. Developers may use
|
||||
// values within that range to identify different branches.
|
||||
//
|
||||
static const uint16_t k_software_type = 10; // 0 for APM trunk
|
||||
|
||||
//
|
||||
// Parameter identities.
|
||||
@ -43,6 +50,7 @@ public:
|
||||
// Layout version number, always key zero.
|
||||
//
|
||||
k_param_format_version = 0,
|
||||
k_param_software_type,
|
||||
|
||||
|
||||
// Misc
|
||||
@ -53,13 +61,13 @@ public:
|
||||
//
|
||||
k_param_streamrates_port0 = 110,
|
||||
k_param_streamrates_port3,
|
||||
k_param_sysid_this_mav,
|
||||
k_param_sysid_my_gcs,
|
||||
|
||||
//
|
||||
// 140: Sensor parameters
|
||||
//
|
||||
k_param_IMU_calibration = 140,
|
||||
k_param_ground_temperature,
|
||||
k_param_ground_pressure,
|
||||
k_param_battery_monitoring,
|
||||
k_param_pack_capacity,
|
||||
k_param_compass_enabled,
|
||||
@ -97,6 +105,7 @@ public:
|
||||
k_param_flight_modes,
|
||||
k_param_esc_calibrate,
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
//
|
||||
// 200: Heli
|
||||
//
|
||||
@ -114,6 +123,7 @@ public:
|
||||
k_param_heli_collective_mid,
|
||||
k_param_heli_ext_gyro_enabled,
|
||||
k_param_heli_ext_gyro_gain, // 213
|
||||
#endif
|
||||
|
||||
//
|
||||
// 220: Waypoint data
|
||||
@ -149,6 +159,12 @@ public:
|
||||
};
|
||||
|
||||
AP_Int16 format_version;
|
||||
AP_Int8 software_type;
|
||||
|
||||
// Telemetry control
|
||||
//
|
||||
AP_Int16 sysid_this_mav;
|
||||
AP_Int16 sysid_my_gcs;
|
||||
|
||||
|
||||
// Crosstrack navigation
|
||||
@ -190,8 +206,6 @@ public:
|
||||
// Misc
|
||||
//
|
||||
AP_Int16 log_bitmask;
|
||||
AP_Int16 ground_temperature;
|
||||
AP_Int16 ground_pressure;
|
||||
AP_Int16 RTL_altitude;
|
||||
|
||||
AP_Int8 sonar_enabled;
|
||||
@ -201,6 +215,7 @@ public:
|
||||
AP_Int8 esc_calibrate;
|
||||
AP_Int8 frame_orientation;
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// Heli
|
||||
RC_Channel heli_servo_1, heli_servo_2, heli_servo_3, heli_servo_4; // servos for swash plate and tail
|
||||
AP_Int16 heli_servo1_pos, heli_servo2_pos, heli_servo3_pos; // servo positions (3 because we don't need pos for tail servo)
|
||||
@ -208,7 +223,8 @@ public:
|
||||
AP_Int16 heli_coll_min, heli_coll_max, heli_coll_mid; // min and max collective. mid = main blades at zero pitch
|
||||
AP_Int8 heli_ext_gyro_enabled; // 0 = no external tail gyro, 1 = external tail gyro
|
||||
AP_Int16 heli_ext_gyro_gain; // radio output 1000~2000 (value output on CH_7)
|
||||
|
||||
#endif
|
||||
|
||||
// RC channels
|
||||
RC_Channel rc_1;
|
||||
RC_Channel rc_2;
|
||||
@ -236,10 +252,15 @@ public:
|
||||
|
||||
uint8_t junk;
|
||||
|
||||
// Note: keep initializers here in the same order as they are declared above.
|
||||
Parameters() :
|
||||
// variable default key name
|
||||
//-------------------------------------------------------------------------------------------------------------------
|
||||
format_version (k_format_version, k_param_format_version, NULL),
|
||||
format_version (k_format_version, k_param_format_version, PSTR("SYSID_SW_MREV")),
|
||||
software_type (k_software_type, k_param_software_type, PSTR("SYSID_SW_TYPE")),
|
||||
|
||||
sysid_this_mav (MAV_SYSTEM_ID, k_param_sysid_this_mav, PSTR("SYSID_THISMAV")),
|
||||
sysid_my_gcs (255, k_param_sysid_my_gcs, PSTR("SYSID_MYGCS")),
|
||||
|
||||
crosstrack_gain (XTRACK_GAIN * 100, k_param_crosstrack_gain, PSTR("XTRK_GAIN")),
|
||||
crosstrack_entry_angle (XTRACK_ENTRY_ANGLE * 100, k_param_crosstrack_entry_angle, PSTR("XTRACK_ANGLE")),
|
||||
@ -268,20 +289,19 @@ public:
|
||||
pitch_max (PITCH_MAX * 100, k_param_pitch_max, PSTR("PITCH_MAX")),
|
||||
|
||||
log_bitmask (MASK_LOG_SET_DEFAULTS, k_param_log_bitmask, PSTR("LOG_BITMASK")),
|
||||
ground_temperature (0, k_param_ground_temperature, PSTR("GND_TEMP")),
|
||||
ground_pressure (0, k_param_ground_pressure, PSTR("GND_ABS_PRESS")),
|
||||
RTL_altitude (ALT_HOLD_HOME * 100, k_param_RTL_altitude, PSTR("ALT_HOLD_RTL")),
|
||||
esc_calibrate (0, k_param_esc_calibrate, PSTR("ESC")),
|
||||
|
||||
frame_orientation (FRAME_ORIENTATION, k_param_frame_orientation, PSTR("FRAME")),
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
heli_servo_1 (k_param_heli_servo_1, PSTR("HS1_")),
|
||||
heli_servo_2 (k_param_heli_servo_2, PSTR("HS2_")),
|
||||
heli_servo_3 (k_param_heli_servo_3, PSTR("HS3_")),
|
||||
heli_servo_4 (k_param_heli_servo_4, PSTR("HS4_")),
|
||||
heli_servo1_pos (-60, k_param_heli_servo1_pos, PSTR("SV1_POS_")),
|
||||
heli_servo2_pos (60, k_param_heli_servo2_pos, PSTR("SV2_POS_")),
|
||||
heli_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")),
|
||||
heli_servo3_pos (180, k_param_heli_servo3_pos, PSTR("SV3_POS_")),
|
||||
heli_roll_max (4500, k_param_heli_roll_max, PSTR("ROL_MAX_")),
|
||||
heli_pitch_max (4500, k_param_heli_pitch_max, PSTR("PIT_MAX_")),
|
||||
heli_coll_min (1000, k_param_heli_collective_min, PSTR("COL_MIN_")),
|
||||
@ -289,7 +309,8 @@ public:
|
||||
heli_coll_mid (1500, k_param_heli_collective_mid, PSTR("COL_MID_")),
|
||||
heli_ext_gyro_enabled (0, k_param_heli_ext_gyro_enabled, PSTR("GYR_ENABLE_")),
|
||||
heli_ext_gyro_gain (1000, k_param_heli_ext_gyro_gain, PSTR("GYR_GAIN_")),
|
||||
|
||||
#endif
|
||||
|
||||
// RC channel group key name
|
||||
//----------------------------------------------------------------------
|
||||
rc_1 (k_param_rc_1, PSTR("RC1_")),
|
||||
|
||||
@ -224,7 +224,8 @@ void init_home()
|
||||
home.id = MAV_CMD_NAV_WAYPOINT;
|
||||
home.lng = g_gps->longitude; // Lon * 10**7
|
||||
home.lat = g_gps->latitude; // Lat * 10**7
|
||||
home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid
|
||||
//home.alt = max(g_gps->altitude, 0); // we sometimes get negatives from GPS, not valid
|
||||
home.alt = 0; // this is a test
|
||||
home_is_set = true;
|
||||
|
||||
// to point yaw towards home until we set it with Mavlink
|
||||
|
||||
@ -49,10 +49,15 @@
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Sonar
|
||||
//
|
||||
|
||||
#ifndef SONAR_PIN
|
||||
# define SONAR_PIN AP_RANGEFINDER_PITOT_TUBE
|
||||
#endif
|
||||
|
||||
#ifndef SONAR_TYPE
|
||||
# define SONAR_TYPE MAX_SONAR_XL
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// AIRSPEED_SENSOR
|
||||
@ -352,7 +357,7 @@
|
||||
# define YAW_P 0.4 // increase for more aggressive Yaw Hold, decrease if it's bouncy
|
||||
#endif
|
||||
#ifndef YAW_I
|
||||
# define YAW_I 0.01 // set to .0001 to try and get over user's steady state error caused by poor balance
|
||||
# define YAW_I 0.005 // set to .0001 to try and get over user's steady state error caused by poor balance
|
||||
#endif
|
||||
#ifndef YAW_D
|
||||
# define YAW_D 0.08 // .7 = almost no yaw Trying a lower value to prevent odd behavior
|
||||
@ -366,7 +371,7 @@
|
||||
//
|
||||
// how much to we pitch towards the target
|
||||
#ifndef PITCH_MAX
|
||||
# define PITCH_MAX 24 // degrees
|
||||
# define PITCH_MAX 16 // degrees
|
||||
#endif
|
||||
|
||||
|
||||
@ -404,7 +409,7 @@
|
||||
# define NAV_WP_P 4.0
|
||||
#endif
|
||||
#ifndef NAV_WP_I
|
||||
# define NAV_WP_I 0.15 // leave 0
|
||||
# define NAV_WP_I 0.15 //
|
||||
#endif
|
||||
#ifndef NAV_WP_D
|
||||
# define NAV_WP_D 10 // not sure about at all
|
||||
@ -419,13 +424,13 @@
|
||||
// Throttle control gains
|
||||
//
|
||||
#ifndef THROTTLE_BARO_P
|
||||
# define THROTTLE_BARO_P 0.2 // trying a lower val
|
||||
# define THROTTLE_BARO_P 0.35 // trying a lower val
|
||||
#endif
|
||||
#ifndef THROTTLE_BARO_I
|
||||
# define THROTTLE_BARO_I 0.02 //with 4m error, 12.5s windup
|
||||
# define THROTTLE_BARO_I 0.05 //with 4m error, 12.5s windup
|
||||
#endif
|
||||
#ifndef THROTTLE_BARO_D
|
||||
# define THROTTLE_BARO_D 0.06 //
|
||||
# define THROTTLE_BARO_D 0.3 // upped with filter
|
||||
#endif
|
||||
#ifndef THROTTLE_BARO_IMAX
|
||||
# define THROTTLE_BARO_IMAX 30
|
||||
|
||||
@ -51,37 +51,45 @@ void read_trim_switch()
|
||||
{
|
||||
// switch is engaged
|
||||
if (g.rc_7.control_in > 800){
|
||||
if(trim_flag == false){
|
||||
// called once
|
||||
trim_timer = millis();
|
||||
}
|
||||
|
||||
trim_flag = true;
|
||||
//trim_accel();
|
||||
|
||||
}else{ // switch is disengaged
|
||||
|
||||
if(trim_flag){
|
||||
// switch was just released
|
||||
if((millis() - trim_timer) > 2000){
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
imu.save();
|
||||
#endif
|
||||
}else{
|
||||
// set the throttle nominal
|
||||
if(g.rc_3.control_in > 150){
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
// set the throttle nominal
|
||||
if(g.rc_3.control_in > 150){
|
||||
g.throttle_cruise.set_and_save(g.rc_3.control_in);
|
||||
//Serial.printf("tnom %d\n", g.throttle_cruise.get());
|
||||
}
|
||||
}
|
||||
trim_flag = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void auto_trim()
|
||||
{
|
||||
if(auto_level_counter > 0){
|
||||
auto_level_counter--;
|
||||
trim_accel();
|
||||
led_mode = AUTO_TRIM_LEDS;
|
||||
|
||||
if(auto_level_counter == 1){
|
||||
led_mode = NORMAL_LEDS;
|
||||
clear_leds();
|
||||
imu.save();
|
||||
Serial.println("Done");
|
||||
auto_level_counter = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void trim_accel()
|
||||
{
|
||||
g.pid_stabilize_roll.reset_I();
|
||||
g.pid_stabilize_pitch.reset_I();
|
||||
|
||||
if(g.rc_1.control_in > 0){
|
||||
imu.ay(imu.ay() + 1);
|
||||
}else if (g.rc_1.control_in < 0){
|
||||
@ -94,11 +102,13 @@ void trim_accel()
|
||||
imu.ax(imu.ax() - 1);
|
||||
}
|
||||
|
||||
/*Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"),
|
||||
/*
|
||||
Serial.printf_P(PSTR("r:%ld p:%ld ax:%f, ay:%f, az:%f\n"),
|
||||
dcm.roll_sensor,
|
||||
dcm.pitch_sensor,
|
||||
(double)imu.ax(),
|
||||
(double)imu.ay(),
|
||||
(double)imu.az());*/
|
||||
(float)imu.ax(),
|
||||
(float)imu.ay(),
|
||||
(float)imu.az());
|
||||
//*/
|
||||
}
|
||||
|
||||
|
||||
@ -25,6 +25,11 @@
|
||||
#define PLUS_FRAME 0
|
||||
#define X_FRAME 1
|
||||
|
||||
|
||||
// LED output
|
||||
#define NORMAL_LEDS 0
|
||||
#define AUTO_TRIM_LEDS 1
|
||||
|
||||
// Internal defines, don't edit and expect things to work
|
||||
// -------------------------------------------------------
|
||||
|
||||
@ -50,6 +55,11 @@
|
||||
#define GPS_PROTOCOL_MTK16 6
|
||||
#define GPS_PROTOCOL_AUTO 7
|
||||
|
||||
// SONAR types:
|
||||
|
||||
#define MAX_SONAR_XL 0
|
||||
#define MAX_SONAR_LV 1
|
||||
|
||||
// Radio channels
|
||||
// Note channels are from 0!
|
||||
//
|
||||
@ -121,10 +131,12 @@
|
||||
#define CH6_SONAR_KD 6
|
||||
#define CH6_Y6_SCALING 7
|
||||
#define CH6_PMAX 8
|
||||
#define CH6_YAW_KP 9
|
||||
#define CH6_YAW_KD 10
|
||||
#define CH6_YAW_RATE_KP 11
|
||||
#define CH6_YAW_RATE_KD 12
|
||||
#define CH6_DCM_RP 9
|
||||
#define CH6_DCM_Y 10
|
||||
#define CH6_YAW_KP 11
|
||||
#define CH6_YAW_KD 12
|
||||
#define CH6_YAW_RATE_KP 13
|
||||
#define CH6_YAW_RATE_KD 14
|
||||
|
||||
// nav byte mask
|
||||
// -------------
|
||||
@ -225,7 +237,8 @@
|
||||
#define SEVERITY_CRITICAL 4
|
||||
|
||||
// Logging parameters
|
||||
#define LOG_INDEX_MSG 0xF0
|
||||
#define TYPE_AIRSTART_MSG 0x00
|
||||
#define TYPE_GROUNDSTART_MSG 0x01
|
||||
#define LOG_ATTITUDE_MSG 0x01
|
||||
#define LOG_GPS_MSG 0x02
|
||||
#define LOG_MODE_MSG 0X03
|
||||
@ -236,8 +249,8 @@
|
||||
#define LOG_CMD_MSG 0x08
|
||||
#define LOG_CURRENT_MSG 0x09
|
||||
#define LOG_STARTUP_MSG 0x0A
|
||||
#define TYPE_AIRSTART_MSG 0x00
|
||||
#define TYPE_GROUNDSTART_MSG 0x01
|
||||
#define LOG_MOTORS_MSG 0x0B
|
||||
#define LOG_INDEX_MSG 0xF0
|
||||
#define MAX_NUM_LOGS 50
|
||||
|
||||
#define MASK_LOG_ATTITUDE_FAST (1<<0)
|
||||
@ -250,6 +263,7 @@
|
||||
#define MASK_LOG_RAW (1<<7)
|
||||
#define MASK_LOG_CMD (1<<8)
|
||||
#define MASK_LOG_CURRENT (1<<9)
|
||||
#define MASK_LOG_MOTORS (1<<10)
|
||||
#define MASK_LOG_SET_DEFAULTS (1<<15)
|
||||
|
||||
// Waypoint Modes
|
||||
@ -276,6 +290,7 @@
|
||||
|
||||
#define BATTERY_VOLTAGE(x) (x*(INPUT_VOLTAGE/1024.0))*VOLT_DIV_RATIO
|
||||
#define CURRENT_AMPS(x) ((x*(INPUT_VOLTAGE/1024.0))-CURR_AMPS_OFFSET)*CURR_AMP_PER_VOLT
|
||||
#define BARO_FILTER_SIZE 6
|
||||
|
||||
/* ************************************************************** */
|
||||
/* Expansion PIN's that people can use for various things. */
|
||||
@ -319,6 +334,11 @@
|
||||
#define PIEZO_PIN AN5 //Last pin on the back ADC connector
|
||||
|
||||
|
||||
#define SONAR_PORT AP_RANGEFINDER_PITOT_TUBE_ADC_CHANNEL
|
||||
|
||||
// sonar
|
||||
#define SonarToCm(x) (x*1.26) // Sonar raw value to centimeters
|
||||
|
||||
// Hardware Parameters
|
||||
#define SLIDE_SWITCH_PIN 40
|
||||
#define PUSHBUTTON_PIN 41
|
||||
|
||||
110
ArduCopterMega/leds.pde
Normal file
110
ArduCopterMega/leds.pde
Normal file
@ -0,0 +1,110 @@
|
||||
void update_lights()
|
||||
{
|
||||
switch(led_mode){
|
||||
case NORMAL_LEDS:
|
||||
update_motor_light();
|
||||
update_GPS_light();
|
||||
break;
|
||||
|
||||
case AUTO_TRIM_LEDS:
|
||||
dancing_light();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void update_GPS_light(void)
|
||||
{
|
||||
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
|
||||
// ---------------------------------------------------------------------
|
||||
switch (g_gps->status()){
|
||||
|
||||
case(2):
|
||||
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
|
||||
break;
|
||||
|
||||
case(1):
|
||||
if (g_gps->valid_read == true){
|
||||
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
|
||||
if (GPS_light){
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
}else{
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
}
|
||||
g_gps->valid_read = false;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void update_motor_light(void)
|
||||
{
|
||||
if(motor_armed == false){
|
||||
motor_light = !motor_light;
|
||||
|
||||
// blink
|
||||
if(motor_light){
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
}else{
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
}
|
||||
}else{
|
||||
if(!motor_light){
|
||||
motor_light = true;
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void dancing_light()
|
||||
{
|
||||
static byte step;
|
||||
|
||||
if (step++ == 3)
|
||||
step = 0;
|
||||
|
||||
switch(step)
|
||||
{
|
||||
case 0:
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(B_LED_PIN, HIGH);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
digitalWrite(B_LED_PIN, LOW);
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
break;
|
||||
}
|
||||
}
|
||||
void clear_leds()
|
||||
{
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(B_LED_PIN, LOW);
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
}
|
||||
|
||||
#if MOTOR_LEDS == 1
|
||||
void update_motor_leds(void)
|
||||
{
|
||||
// blink rear
|
||||
static bool blink;
|
||||
|
||||
if (blink){
|
||||
blink = false;
|
||||
digitalWrite(RE_LED, LOW);
|
||||
|
||||
}else{
|
||||
blink = true;
|
||||
digitalWrite(RE_LED, HIGH);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -1,17 +1,27 @@
|
||||
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#define ARM_DELAY 10
|
||||
#define DISARM_DELAY 10
|
||||
#define ARM_DELAY 10 // one secon
|
||||
#define DISARM_DELAY 10 // one secon
|
||||
#define LEVEL_DELAY 120 // twelve seconds
|
||||
#define AUTO_LEVEL_DELAY 250 // twentyfive seconds
|
||||
|
||||
|
||||
// called at 10hz
|
||||
void arm_motors()
|
||||
{
|
||||
static byte arming_counter;
|
||||
static int arming_counter;
|
||||
|
||||
// Arm motor output : Throttle down and full yaw right for more than 2 seconds
|
||||
if (g.rc_3.control_in == 0){
|
||||
|
||||
// full right
|
||||
if (g.rc_4.control_in > 4000) {
|
||||
if (arming_counter >= ARM_DELAY) {
|
||||
|
||||
if (arming_counter > AUTO_LEVEL_DELAY){
|
||||
auto_level_counter = 255;
|
||||
arming_counter = 0;
|
||||
|
||||
}else if (arming_counter == ARM_DELAY){
|
||||
motor_armed = true;
|
||||
arming_counter = ARM_DELAY;
|
||||
|
||||
@ -19,16 +29,24 @@ void arm_motors()
|
||||
// ---------------------------
|
||||
init_simple_bearing();
|
||||
|
||||
arming_counter++;
|
||||
} else{
|
||||
arming_counter++;
|
||||
}
|
||||
|
||||
// full left
|
||||
}else if (g.rc_4.control_in < -4000) {
|
||||
if (arming_counter >= DISARM_DELAY){
|
||||
//Serial.print(arming_counter, DEC);
|
||||
if (arming_counter > LEVEL_DELAY){
|
||||
//Serial.print("init");
|
||||
imu.init_accel();
|
||||
arming_counter = 0;
|
||||
|
||||
}else if (arming_counter == DISARM_DELAY){
|
||||
motor_armed = false;
|
||||
arming_counter = DISARM_DELAY;
|
||||
compass.save_offsets();
|
||||
arming_counter++;
|
||||
|
||||
}else{
|
||||
arming_counter++;
|
||||
|
||||
@ -22,7 +22,7 @@ void init_rc_in()
|
||||
g.rc_1.dead_zone = 60; // 60 = .6 degrees
|
||||
g.rc_2.dead_zone = 60;
|
||||
g.rc_3.dead_zone = 60;
|
||||
g.rc_4.dead_zone = 300;
|
||||
g.rc_4.dead_zone = 600;
|
||||
|
||||
//set auxiliary ranges
|
||||
g.rc_5.set_range(0,1000);
|
||||
@ -106,29 +106,32 @@ void output_min()
|
||||
}
|
||||
void read_radio()
|
||||
{
|
||||
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
|
||||
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
|
||||
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
|
||||
g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
|
||||
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
|
||||
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
|
||||
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
|
||||
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
|
||||
if (APM_RC.GetState() == 1){
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
// limit our input to 800 so we can still pitch and roll
|
||||
g.rc_3.control_in = min(g.rc_3.control_in, 800);
|
||||
#endif
|
||||
g.rc_1.set_pwm(APM_RC.InputCh(CH_1));
|
||||
g.rc_2.set_pwm(APM_RC.InputCh(CH_2));
|
||||
g.rc_3.set_pwm(APM_RC.InputCh(CH_3));
|
||||
g.rc_4.set_pwm(APM_RC.InputCh(CH_4));
|
||||
g.rc_5.set_pwm(APM_RC.InputCh(CH_5));
|
||||
g.rc_6.set_pwm(APM_RC.InputCh(CH_6));
|
||||
g.rc_7.set_pwm(APM_RC.InputCh(CH_7));
|
||||
g.rc_8.set_pwm(APM_RC.InputCh(CH_8));
|
||||
|
||||
//throttle_failsafe(g.rc_3.radio_in);
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
// limit our input to 800 so we can still pitch and roll
|
||||
g.rc_3.control_in = min(g.rc_3.control_in, 800);
|
||||
#endif
|
||||
|
||||
/*
|
||||
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
|
||||
g.rc_1.control_in,
|
||||
g.rc_2.control_in,
|
||||
g.rc_3.control_in,
|
||||
g.rc_4.control_in);
|
||||
*/
|
||||
//throttle_failsafe(g.rc_3.radio_in);
|
||||
|
||||
/*
|
||||
Serial.printf_P(PSTR("OUT 1: %d\t2: %d\t3: %d\t4: %d \n"),
|
||||
g.rc_1.control_in,
|
||||
g.rc_2.control_in,
|
||||
g.rc_3.control_in,
|
||||
g.rc_4.control_in);
|
||||
*/
|
||||
}
|
||||
}
|
||||
|
||||
void throttle_failsafe(uint16_t pwm)
|
||||
|
||||
@ -7,66 +7,65 @@ void ReadSCP1000(void) {}
|
||||
|
||||
void init_barometer(void)
|
||||
{
|
||||
int flashcount;
|
||||
|
||||
#if HIL_MODE == HIL_MODE_SENSORS
|
||||
hil.update(); // look for inbound hil packets for initialization
|
||||
#endif
|
||||
|
||||
ground_pressure = 0;
|
||||
|
||||
while(ground_pressure == 0){
|
||||
barometer.Read(); // Get initial data from absolute pressure sensor
|
||||
ground_pressure = barometer.Press;
|
||||
ground_temperature = barometer.Temp;
|
||||
delay(20);
|
||||
//Serial.printf("barometer.Press %ld\n", barometer.Press);
|
||||
}
|
||||
|
||||
for(int i = 0; i < 30; i++){ // We take some readings...
|
||||
// We take some readings...
|
||||
for(int i = 0; i < 200; i++){
|
||||
delay(25);
|
||||
|
||||
#if HIL_MODE == HIL_MODE_SENSORS
|
||||
hil.update(); // look for inbound hil packets
|
||||
#endif
|
||||
|
||||
barometer.Read(); // Get initial data from absolute pressure sensor
|
||||
ground_pressure = (ground_pressure * 9l + barometer.Press) / 10l;
|
||||
// Get initial data from absolute pressure sensor
|
||||
ground_pressure = read_baro_filtered();
|
||||
ground_temperature = (ground_temperature * 9 + barometer.Temp) / 10;
|
||||
|
||||
delay(20);
|
||||
if(flashcount == 5) {
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
digitalWrite(B_LED_PIN, LOW);
|
||||
}
|
||||
}
|
||||
abs_pressure = ground_pressure;
|
||||
ground_temperature = barometer.Temp;
|
||||
|
||||
if(flashcount >= 10) {
|
||||
flashcount = 0;
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(B_LED_PIN, HIGH);
|
||||
}
|
||||
flashcount++;
|
||||
//Serial.printf("abs_pressure %ld\n", ground_temperature);
|
||||
//SendDebugln("barometer calibration complete.");
|
||||
}
|
||||
|
||||
long read_baro_filtered(void)
|
||||
{
|
||||
long pressure = 0;
|
||||
|
||||
// get new data from absolute pressure sensor
|
||||
barometer.Read();
|
||||
|
||||
// add new data into our filter
|
||||
baro_filter[baro_filter_index] = barometer.Press;
|
||||
baro_filter_index++;
|
||||
|
||||
// loop our filter
|
||||
if(baro_filter_index == BARO_FILTER_SIZE)
|
||||
baro_filter_index = 0;
|
||||
|
||||
// zero out our accumulator
|
||||
|
||||
// sum our filter
|
||||
for(byte i = 0; i < BARO_FILTER_SIZE; i++){
|
||||
pressure += baro_filter[i];
|
||||
}
|
||||
|
||||
// makes the filtering work later
|
||||
abs_pressure = barometer.Press;
|
||||
|
||||
// save home pressure
|
||||
ground_pressure = abs_pressure;
|
||||
|
||||
//Serial.printf("abs_pressure %ld\n", abs_pressure);
|
||||
//SendDebugln("barometer calibration complete.");
|
||||
// average our sampels
|
||||
return pressure /= BARO_FILTER_SIZE;
|
||||
}
|
||||
|
||||
long read_barometer(void)
|
||||
{
|
||||
float x, scaling, temp;
|
||||
|
||||
barometer.Read(); // Get new data from absolute pressure sensor
|
||||
abs_pressure = read_baro_filtered();
|
||||
|
||||
//Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure);
|
||||
|
||||
//abs_pressure = (abs_pressure + barometer.Press) >> 1; // Small filtering
|
||||
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.Press * .3); // large filtering
|
||||
scaling = (float)ground_pressure / (float)abs_pressure;
|
||||
temp = ((float)ground_temperature / 10.0f) + 273.15f;
|
||||
x = log(scaling) * temp * 29271.267f;
|
||||
|
||||
@ -15,9 +15,10 @@ static int8_t setup_mag_offset (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_esc (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
static int8_t setup_heli (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_heli (uint8_t argc, const Menu::arg *argv);
|
||||
static int8_t setup_gyro (uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
|
||||
// Command/function table for the setup menu
|
||||
@ -37,7 +38,7 @@ const struct Menu::command setup_menu_commands[] PROGMEM = {
|
||||
{"compass", setup_compass},
|
||||
{"offsets", setup_mag_offset},
|
||||
{"declination", setup_declination},
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
{"heli", setup_heli},
|
||||
{"gyro", setup_gyro},
|
||||
#endif
|
||||
@ -239,7 +240,7 @@ init_esc()
|
||||
while(1){
|
||||
read_radio();
|
||||
delay(100);
|
||||
update_esc_light();
|
||||
dancing_light();
|
||||
APM_RC.OutputCh(CH_1, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_2, g.rc_3.radio_in);
|
||||
APM_RC.OutputCh(CH_3, g.rc_3.radio_in);
|
||||
@ -300,7 +301,9 @@ setup_frame(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
byte switchPosition, _oldSwitchPosition, mode;
|
||||
byte _switchPosition = 0;
|
||||
byte _oldSwitchPosition = 0;
|
||||
byte mode = 0;
|
||||
|
||||
Serial.printf_P(PSTR("\nMove RC toggle switch to each position to edit, move aileron stick to select modes."));
|
||||
print_hit_enter();
|
||||
@ -308,20 +311,20 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
while(1){
|
||||
delay(20);
|
||||
read_radio();
|
||||
switchPosition = readSwitch();
|
||||
_switchPosition = readSwitch();
|
||||
|
||||
|
||||
// look for control switch change
|
||||
if (_oldSwitchPosition != switchPosition){
|
||||
if (_oldSwitchPosition != _switchPosition){
|
||||
|
||||
mode = g.flight_modes[switchPosition];
|
||||
mode = g.flight_modes[_switchPosition];
|
||||
mode = constrain(mode, 0, NUM_MODES-1);
|
||||
|
||||
// update the user
|
||||
print_switch(switchPosition, mode);
|
||||
print_switch(_switchPosition, mode);
|
||||
|
||||
// Remember switch position
|
||||
_oldSwitchPosition = switchPosition;
|
||||
_oldSwitchPosition = _switchPosition;
|
||||
}
|
||||
|
||||
// look for stick input
|
||||
@ -331,10 +334,10 @@ setup_flightmodes(uint8_t argc, const Menu::arg *argv)
|
||||
mode = 0;
|
||||
|
||||
// save new mode
|
||||
g.flight_modes[switchPosition] = mode;
|
||||
g.flight_modes[_switchPosition] = mode;
|
||||
|
||||
// print new mode
|
||||
print_switch(switchPosition, mode);
|
||||
print_switch(_switchPosition, mode);
|
||||
}
|
||||
|
||||
// escape hatch
|
||||
@ -352,6 +355,7 @@ setup_declination(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
compass.set_declination(radians(argv[1].f));
|
||||
report_compass();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
@ -436,13 +440,13 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
// initialise swash plate
|
||||
heli_init_swash();
|
||||
|
||||
// source swash plate movements directly from
|
||||
|
||||
// source swash plate movements directly from
|
||||
heli_manual_override = true;
|
||||
|
||||
|
||||
// display initial settings
|
||||
report_heli();
|
||||
|
||||
|
||||
// display help
|
||||
Serial.printf_P(PSTR("Instructions:"));
|
||||
print_divider();
|
||||
@ -455,14 +459,14 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
Serial.printf_P(PSTR("\tr\t\treverse servo\n"));
|
||||
Serial.printf_P(PSTR("\tt<angle>\tset trim (-500 ~ 500)\n"));
|
||||
Serial.printf_P(PSTR("\tx\t\texit & save\n"));
|
||||
|
||||
|
||||
// start capturing
|
||||
while( value != 'x' ) {
|
||||
|
||||
|
||||
// read radio although we don't use it yet
|
||||
read_radio();
|
||||
|
||||
// record min/max
|
||||
|
||||
// record min/max
|
||||
if( state == 1 ) {
|
||||
if( abs(g.rc_1.control_in) > max_roll )
|
||||
max_roll = abs(g.rc_1.control_in);
|
||||
@ -476,10 +480,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
max_tail = max(g.rc_4.radio_in, max_tail);
|
||||
//Serial.printf_P(PSTR("4: ri:%d \tro:%d \tso:%d \n"), (int)g.rc_4.radio_in, (int)g.rc_4.radio_out, (int)g.rc_4.servo_out);
|
||||
}
|
||||
|
||||
|
||||
if( Serial.available() ) {
|
||||
value = Serial.read();
|
||||
|
||||
|
||||
// process the user's input
|
||||
switch( value ) {
|
||||
case '1':
|
||||
@ -494,7 +498,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
case '4':
|
||||
active_servo = CH_4;
|
||||
break;
|
||||
case 'a':
|
||||
case 'a':
|
||||
case 'A':
|
||||
heli_get_servo(active_servo)->radio_trim += 10;
|
||||
break;
|
||||
@ -515,14 +519,14 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
if( state == 0 ) {
|
||||
state = 1; // switch to capture min/max mode
|
||||
Serial.printf_P(PSTR("Move coll, roll, pitch and tail to extremes, press 'm' when done\n"),active_servo+1, temp);
|
||||
|
||||
|
||||
// reset servo ranges
|
||||
g.heli_roll_max = g.heli_pitch_max = 4500;
|
||||
g.heli_coll_min = 1000;
|
||||
g.heli_coll_max = 2000;
|
||||
g.heli_servo_4.radio_min = 1000;
|
||||
g.heli_servo_4.radio_max = 2000;
|
||||
|
||||
|
||||
// set sensible values in temp variables
|
||||
max_roll = abs(g.rc_1.control_in);
|
||||
max_pitch = abs(g.rc_2.control_in);
|
||||
@ -541,10 +545,10 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
g.heli_coll_max = max_coll;
|
||||
g.heli_servo_4.radio_min = min_tail;
|
||||
g.heli_servo_4.radio_max = max_tail;
|
||||
|
||||
|
||||
// reinitialise swash
|
||||
heli_init_swash();
|
||||
|
||||
|
||||
// display settings
|
||||
report_heli();
|
||||
}
|
||||
@ -561,7 +565,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
if( active_servo == CH_3 )
|
||||
g.heli_servo3_pos = temp;
|
||||
heli_init_swash();
|
||||
Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
|
||||
Serial.printf_P(PSTR("Servo %d\t\tpos:%d\n"),active_servo+1, temp);
|
||||
}
|
||||
break;
|
||||
case 'r':
|
||||
@ -576,25 +580,25 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
if( temp > -500 && temp < 500 ) {
|
||||
heli_get_servo(active_servo)->radio_trim = 1500 + temp;
|
||||
heli_init_swash();
|
||||
Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
|
||||
Serial.printf_P(PSTR("Servo %d\t\ttrim:%d\n"),active_servo+1, 1500 + temp);
|
||||
}
|
||||
break;
|
||||
case 'z':
|
||||
case 'z':
|
||||
case 'Z':
|
||||
heli_get_servo(active_servo)->radio_trim -= 10;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// allow swash plate to move
|
||||
output_motors_armed();
|
||||
|
||||
|
||||
delay(20);
|
||||
}
|
||||
|
||||
// display final settings
|
||||
report_heli();
|
||||
|
||||
|
||||
// save to eeprom
|
||||
g.heli_servo_1.save_eeprom();
|
||||
g.heli_servo_2.save_eeprom();
|
||||
@ -608,7 +612,7 @@ setup_heli(uint8_t argc, const Menu::arg *argv)
|
||||
g.heli_coll_min.save();
|
||||
g.heli_coll_max.save();
|
||||
g.heli_coll_mid.save();
|
||||
|
||||
|
||||
// return swash plate movements to attitude controller
|
||||
heli_manual_override = false;
|
||||
|
||||
@ -627,7 +631,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
|
||||
g.heli_ext_gyro_gain = argv[2].i;
|
||||
g.heli_ext_gyro_gain.save();
|
||||
}
|
||||
|
||||
|
||||
} else if (!strcmp_P(argv[1].str, PSTR("off"))) {
|
||||
g.heli_ext_gyro_enabled.set_and_save(false);
|
||||
|
||||
@ -646,6 +650,7 @@ setup_gyro(uint8_t argc, const Menu::arg *argv)
|
||||
}
|
||||
|
||||
#endif // FRAME_CONFIG == HELI
|
||||
|
||||
void clear_offsets()
|
||||
{
|
||||
Vector3f _offsets;
|
||||
@ -710,7 +715,7 @@ setup_mag_offset(uint8_t argc, const Menu::arg *argv)
|
||||
compass.set_offsets(_offsets);
|
||||
//compass.set_offsets(mag_offset_x, mag_offset_y, mag_offset_z);
|
||||
report_compass();
|
||||
break;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -959,18 +964,18 @@ void report_heli()
|
||||
{
|
||||
Serial.printf_P(PSTR("Heli\n"));
|
||||
print_divider();
|
||||
|
||||
|
||||
// main servo settings
|
||||
Serial.printf_P(PSTR("Servo \tpos \tmin \tmax \trev\n"));
|
||||
Serial.printf_P(PSTR("1:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo1_pos, (int)g.heli_servo_1.radio_min, (int)g.heli_servo_1.radio_max, (int)g.heli_servo_1.get_reverse());
|
||||
Serial.printf_P(PSTR("2:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo2_pos, (int)g.heli_servo_2.radio_min, (int)g.heli_servo_2.radio_max, (int)g.heli_servo_2.get_reverse());
|
||||
Serial.printf_P(PSTR("3:\t%d \t%d \t%d \t%d\n"),(int)g.heli_servo3_pos, (int)g.heli_servo_3.radio_min, (int)g.heli_servo_3.radio_max, (int)g.heli_servo_3.get_reverse());
|
||||
Serial.printf_P(PSTR("tail:\t\t%d \t%d \t%d\n"), (int)g.heli_servo_4.radio_min, (int)g.heli_servo_4.radio_max, (int)g.heli_servo_4.get_reverse());
|
||||
|
||||
|
||||
Serial.printf_P(PSTR("roll max: \t%d\n"), (int)g.heli_roll_max);
|
||||
Serial.printf_P(PSTR("pitch max: \t%d\n"), (int)g.heli_pitch_max);
|
||||
Serial.printf_P(PSTR("coll min:\t%d\t mid:%d\t max:%d\n"),(int)g.heli_coll_min, (int)g.heli_coll_mid, (int)g.heli_coll_max);
|
||||
|
||||
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
@ -983,7 +988,7 @@ void report_gyro()
|
||||
print_enabled( g.heli_ext_gyro_enabled );
|
||||
if( g.heli_ext_gyro_enabled )
|
||||
Serial.printf_P(PSTR("gain: %d"),(int)g.heli_ext_gyro_gain);
|
||||
|
||||
|
||||
print_blanks(2);
|
||||
}
|
||||
|
||||
@ -1076,11 +1081,14 @@ radio_input_switch(void)
|
||||
|
||||
void zero_eeprom(void)
|
||||
{
|
||||
byte b;
|
||||
byte b = 0;
|
||||
|
||||
Serial.printf_P(PSTR("\nErasing EEPROM\n"));
|
||||
|
||||
for (int i = 0; i < EEPROM_MAX_ADDR; i++) {
|
||||
eeprom_write_byte((uint8_t *) i, b);
|
||||
}
|
||||
|
||||
Serial.printf_P(PSTR("done\n"));
|
||||
}
|
||||
|
||||
@ -1111,6 +1119,8 @@ print_gyro_offsets(void)
|
||||
(float)imu.gz());
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
|
||||
RC_Channel *
|
||||
heli_get_servo(int servo_num){
|
||||
if( servo_num == CH_1 )
|
||||
@ -1140,6 +1150,7 @@ int read_num_from_serial() {
|
||||
index++;
|
||||
}
|
||||
}while (timeout < 5 && index < 5);
|
||||
|
||||
|
||||
return atoi(data);
|
||||
}
|
||||
#endif
|
||||
@ -37,7 +37,7 @@ const struct Menu::command main_menu_commands[] PROGMEM = {
|
||||
};
|
||||
|
||||
// Create the top-level menu object.
|
||||
MENU(main_menu, "AC 2.0.23 Beta", main_menu_commands);
|
||||
MENU(main_menu, "AC 2.0.24 Beta", main_menu_commands);
|
||||
|
||||
void init_ardupilot()
|
||||
{
|
||||
@ -87,6 +87,23 @@ void init_ardupilot()
|
||||
//
|
||||
report_version();
|
||||
|
||||
|
||||
// setup IO pins
|
||||
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
|
||||
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
|
||||
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
|
||||
pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode
|
||||
pinMode(PUSHBUTTON_PIN, INPUT); // unused
|
||||
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
|
||||
|
||||
#if MOTOR_LEDS == 1
|
||||
pinMode(FR_LED, OUTPUT); // GPS status LED
|
||||
pinMode(RE_LED, OUTPUT); // GPS status LED
|
||||
pinMode(RI_LED, OUTPUT); // GPS status LED
|
||||
pinMode(LE_LED, OUTPUT); // GPS status LED
|
||||
#endif
|
||||
|
||||
|
||||
if (!g.format_version.load() ||
|
||||
g.format_version != Parameters::k_format_version) {
|
||||
//Serial.printf_P(PSTR("\n\nForcing complete parameter reset..."));
|
||||
@ -134,10 +151,10 @@ void init_ardupilot()
|
||||
APM_RC.setHIL(rc_override);
|
||||
}
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
heli_init_swash(); // heli initialisation
|
||||
#endif
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
heli_init_swash(); // heli initialisation
|
||||
#endif
|
||||
|
||||
init_rc_in(); // sets up rc channels from radio
|
||||
init_rc_out(); // sets up the timer libs
|
||||
@ -147,8 +164,8 @@ void init_ardupilot()
|
||||
#endif
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
adc.Init(); // APM ADC library initialization
|
||||
barometer.Init(); // APM Abs Pressure sensor initialization
|
||||
adc.Init(); // APM ADC library initialization
|
||||
barometer.Init(); // APM Abs Pressure sensor initialization
|
||||
#endif
|
||||
|
||||
// Do GPS init
|
||||
@ -164,21 +181,20 @@ void init_ardupilot()
|
||||
#endif
|
||||
|
||||
// init the HIL
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
#if HIL_MODE != HIL_MODE_DISABLED
|
||||
#if HIL_PORT == 3
|
||||
hil.init(&Serial3);
|
||||
#elif HIL_PORT == 1
|
||||
hil.init(&Serial1);
|
||||
#else
|
||||
hil.init(&Serial);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
#if HIL_PORT == 3
|
||||
hil.init(&Serial3);
|
||||
#elif HIL_PORT == 1
|
||||
hil.init(&Serial1);
|
||||
#else
|
||||
hil.init(&Serial);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// We may have a hil object instantiated just for mission planning
|
||||
#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
|
||||
hil.init(&Serial);
|
||||
#endif
|
||||
// We may have a hil object instantiated just for mission planning
|
||||
#if HIL_MODE == HIL_MODE_DISABLED && HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_PORT == 0
|
||||
hil.init(&Serial);
|
||||
#endif
|
||||
|
||||
if(g.compass_enabled)
|
||||
init_compass();
|
||||
@ -189,23 +205,15 @@ void init_ardupilot()
|
||||
}
|
||||
#endif
|
||||
|
||||
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
|
||||
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
|
||||
pinMode(B_LED_PIN, OUTPUT); // GPS status LED
|
||||
pinMode(SLIDE_SWITCH_PIN, INPUT); // To enter interactive mode
|
||||
pinMode(PUSHBUTTON_PIN, INPUT); // unused
|
||||
DDRL |= B00000100; // Set Port L, pin 2 to output for the relay
|
||||
|
||||
#if MOTOR_LEDS == 1
|
||||
pinMode(FR_LED, OUTPUT); // GPS status LED
|
||||
pinMode(RE_LED, OUTPUT); // GPS status LED
|
||||
pinMode(RI_LED, OUTPUT); // GPS status LED
|
||||
pinMode(LE_LED, OUTPUT); // GPS status LED
|
||||
#endif
|
||||
// setup DCM for copters:
|
||||
dcm.kp_roll_pitch(0.12); // higher for quads
|
||||
dcm.ki_roll_pitch(0.00000319); // 1/4 of the normal rate
|
||||
|
||||
// Logging:
|
||||
// --------
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
// DataFlash log initialization
|
||||
DataFlash.Init();
|
||||
|
||||
// setup the log bitmask
|
||||
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS)
|
||||
default_log_bitmask();
|
||||
@ -216,7 +224,7 @@ void init_ardupilot()
|
||||
// the system in an odd state, we don't let the user exit the top
|
||||
// menu; they must reset in order to fly.
|
||||
//
|
||||
if (digitalRead(SLIDE_SWITCH_PIN) == 0) {
|
||||
if (check_startup_for_CLI()) {
|
||||
digitalWrite(A_LED_PIN,HIGH); // turn on setup-mode LED
|
||||
Serial.printf_P(PSTR("\n"
|
||||
"Entering interactive setup mode...\n"
|
||||
@ -233,6 +241,17 @@ void init_ardupilot()
|
||||
}
|
||||
}
|
||||
|
||||
// Logging:
|
||||
// --------
|
||||
if(g.log_bitmask != 0){
|
||||
// TODO - Here we will check on the length of the last log
|
||||
// We don't want to create a bunch of little logs due to powering on and off
|
||||
start_new_log();
|
||||
}
|
||||
|
||||
//if (g.log_bitmask & MASK_LOG_MODE)
|
||||
// Log_Write_Mode(control_mode);
|
||||
|
||||
// All of the Gyro calibrations
|
||||
// ----------------------------
|
||||
startup_ground();
|
||||
@ -244,16 +263,9 @@ void init_ardupilot()
|
||||
// init the Yaw Hold output
|
||||
clear_yaw_control();
|
||||
|
||||
// Logging:
|
||||
// --------
|
||||
if(g.log_bitmask != 0){
|
||||
// TODO - Here we will check on the length of the last log
|
||||
// We don't want to create a bunch of little logs due to powering on and off
|
||||
start_new_log();
|
||||
}
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_MODE)
|
||||
Log_Write_Mode(control_mode);
|
||||
delay(100);
|
||||
|
||||
}
|
||||
|
||||
//********************************************************************************
|
||||
@ -263,8 +275,6 @@ void startup_ground(void)
|
||||
{
|
||||
gcs.send_text_P(SEVERITY_LOW,PSTR("GROUND START"));
|
||||
|
||||
// make Motor light go dark
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
|
||||
#if(GROUND_START_DELAY > 0)
|
||||
//gcs.send_text_P(SEVERITY_LOW, PSTR(" With Delay"));
|
||||
@ -310,7 +320,13 @@ void startup_ground(void)
|
||||
}
|
||||
}
|
||||
|
||||
clear_leds();
|
||||
|
||||
report_gps();
|
||||
g_gps->idleTimeout = 20000;
|
||||
|
||||
Log_Write_Startup();
|
||||
|
||||
SendDebug("\nReady to FLY ");
|
||||
//gcs.send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to FLY."));
|
||||
}
|
||||
@ -336,6 +352,8 @@ void set_mode(byte mode)
|
||||
//Serial.printf("set mode: %d\n",control_mode);
|
||||
Serial.println(flight_mode_strings[control_mode]);
|
||||
|
||||
led_mode = NORMAL_LEDS;
|
||||
|
||||
switch(control_mode)
|
||||
{
|
||||
case ACRO:
|
||||
@ -402,95 +420,6 @@ void set_failsafe(boolean mode)
|
||||
}
|
||||
}
|
||||
|
||||
#if MOTOR_LEDS == 1
|
||||
void update_motor_leds(void)
|
||||
{
|
||||
// blink rear
|
||||
static bool blink;
|
||||
|
||||
if (blink){
|
||||
blink = false;
|
||||
digitalWrite(RE_LED, LOW);
|
||||
|
||||
}else{
|
||||
blink = true;
|
||||
digitalWrite(RE_LED, HIGH);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void update_GPS_light(void)
|
||||
{
|
||||
// GPS LED on if we have a fix or Blink GPS LED if we are receiving data
|
||||
// ---------------------------------------------------------------------
|
||||
switch (g_gps->status()){
|
||||
|
||||
case(2):
|
||||
digitalWrite(C_LED_PIN, HIGH); //Turn LED C on when gps has valid fix.
|
||||
break;
|
||||
|
||||
case(1):
|
||||
if (g_gps->valid_read == true){
|
||||
GPS_light = !GPS_light; // Toggle light on and off to indicate gps messages being received, but no GPS fix lock
|
||||
if (GPS_light){
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
}else{
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
}
|
||||
g_gps->valid_read = false;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void update_motor_light(void)
|
||||
{
|
||||
if(motor_armed == false){
|
||||
motor_light = !motor_light;
|
||||
|
||||
// blink
|
||||
if(motor_light){
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
}else{
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
}
|
||||
}else{
|
||||
if(!motor_light){
|
||||
motor_light = true;
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void update_esc_light()
|
||||
{
|
||||
static byte step;
|
||||
|
||||
if (step++ == 3)
|
||||
step = 0;
|
||||
|
||||
switch(step)
|
||||
{
|
||||
case 0:
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
digitalWrite(A_LED_PIN, HIGH);
|
||||
break;
|
||||
|
||||
case 1:
|
||||
digitalWrite(A_LED_PIN, LOW);
|
||||
digitalWrite(B_LED_PIN, HIGH);
|
||||
break;
|
||||
|
||||
case 2:
|
||||
digitalWrite(B_LED_PIN, LOW);
|
||||
digitalWrite(C_LED_PIN, HIGH);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void resetPerfData(void) {
|
||||
@ -541,3 +470,27 @@ init_throttle_cruise()
|
||||
//}
|
||||
}
|
||||
|
||||
#if BROKEN_SLIDER == 1
|
||||
|
||||
boolean
|
||||
check_startup_for_CLI()
|
||||
{
|
||||
if(abs(g.rc_4.control_in) > 3000){
|
||||
// startup to fly
|
||||
return true;
|
||||
}else{
|
||||
// CLI mode
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
boolean
|
||||
check_startup_for_CLI()
|
||||
{
|
||||
return (digitalRead(SLIDE_SWITCH_PIN) == 0);
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@ -402,14 +402,12 @@ static int8_t
|
||||
test_adc(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
adc.Init();
|
||||
delay(1000);
|
||||
Serial.printf_P(PSTR("ADC\n"));
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
for(int i = 0; i < 9; i++){
|
||||
Serial.printf_P(PSTR("i:%d\t"),adc.Ch(i));
|
||||
Serial.printf_P(PSTR("%d,"),adc.Ch(i));
|
||||
}
|
||||
Serial.println();
|
||||
delay(20);
|
||||
@ -433,29 +431,14 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
||||
delay(1000);
|
||||
|
||||
//float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
|
||||
|
||||
fast_loopTimer = millis();
|
||||
|
||||
while(1){
|
||||
delay(20);
|
||||
if (millis() - fast_loopTimer > 19) {
|
||||
//delay(20);
|
||||
if (millis() - fast_loopTimer >= 5) {
|
||||
delta_ms_fast_loop = millis() - fast_loopTimer;
|
||||
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
|
||||
fast_loopTimer = millis();
|
||||
/*
|
||||
Matrix3f temp = dcm.get_dcm_matrix();
|
||||
|
||||
sin_pitch = -temp.c.x;
|
||||
cos_pitch = sqrt(1 - (temp.c.x * temp.c.x));
|
||||
|
||||
cos_roll = temp.c.z / cos_pitch;
|
||||
sin_roll = temp.c.y / cos_pitch;
|
||||
|
||||
yawvector.x = temp.a.x; // sin
|
||||
yawvector.y = temp.b.x; // cos
|
||||
yawvector.normalize();
|
||||
cos_yaw = yawvector.y; // 0 x = north
|
||||
sin_yaw = yawvector.x; // 1 y
|
||||
*/
|
||||
|
||||
// IMU
|
||||
// ---
|
||||
@ -464,12 +447,33 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
||||
Vector3f accels = imu.get_accel();
|
||||
Vector3f gyros = imu.get_gyro();
|
||||
|
||||
if(g.compass_enabled){
|
||||
medium_loopCounter++;
|
||||
if(medium_loopCounter == 5){
|
||||
medium_loopCounter++;
|
||||
if(medium_loopCounter == 4){
|
||||
update_trig();
|
||||
}
|
||||
|
||||
if(medium_loopCounter == 20){
|
||||
read_radio();
|
||||
medium_loopCounter = 0;
|
||||
//tuning();
|
||||
//dcm.kp_roll_pitch((float)g.rc_6.control_in / 2000.0);
|
||||
|
||||
/*
|
||||
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld, kp:%1.4f, kp:%1.4f \n"),
|
||||
dcm.roll_sensor,
|
||||
dcm.pitch_sensor,
|
||||
dcm.yaw_sensor,
|
||||
dcm.kp_roll_pitch(),
|
||||
(float)g.rc_6.control_in / 2000.0);
|
||||
*/
|
||||
Serial.printf_P(PSTR("%ld, %ld, %ld\n"),
|
||||
dcm.roll_sensor,
|
||||
dcm.pitch_sensor,
|
||||
dcm.yaw_sensor);
|
||||
|
||||
if(g.compass_enabled){
|
||||
compass.read(); // Read magnetometer
|
||||
compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
|
||||
medium_loopCounter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@ -481,12 +485,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
|
||||
accels.x, accels.y, accels.z,
|
||||
gyros.x, gyros.y, gyros.z);
|
||||
*/
|
||||
Serial.printf_P(PSTR("r: %ld\tp: %ld\t y: %ld\n"),
|
||||
dcm.roll_sensor,
|
||||
dcm.pitch_sensor,
|
||||
dcm.yaw_sensor);
|
||||
/*
|
||||
update_trig();
|
||||
Serial.printf_P(PSTR("cp: %1.2f, sp: %1.2f, cr: %1.2f, sr: %1.2f, cy: %1.2f, sy: %1.2f,\n"),
|
||||
cos_pitch_x,
|
||||
sin_pitch_y,
|
||||
@ -699,23 +698,30 @@ test_tuning(uint8_t argc, const Menu::arg *argv)
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_Y6_SCALING
|
||||
Serial.printf_P(PSTR("Y6: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_DCM_RP
|
||||
Serial.printf_P(PSTR("DCM RP: %1.4f\n"), ((float)g.rc_6.control_in / 5000.0));
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_DCM_Y
|
||||
Serial.printf_P(PSTR("DCM Y: %1.4f\n"), ((float)g.rc_6.control_in / 1000.0));
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_KP
|
||||
// yaw heading
|
||||
Serial.printf_P(PSTR("yaw kP: %1.3f\n"), ((float)g.rc_6.control_in / 200.0)); // range from 0 ~ 5.0
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_KD
|
||||
// yaw heading
|
||||
Serial.printf_P(PSTR("yaw kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KP
|
||||
// yaw rate
|
||||
Serial.printf_P(PSTR("yaw rate kP: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
||||
|
||||
|
||||
#elif CHANNEL_6_TUNING == CH6_YAW_RATE_KD
|
||||
// yaw rate
|
||||
Serial.printf_P(PSTR("yaw rate kD: %1.3f\n"), ((float)g.rc_6.control_in / 1000.0));
|
||||
#endif
|
||||
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
@ -796,23 +802,18 @@ test_wp(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
test_rawgps(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
print_hit_enter();
|
||||
delay(1000);
|
||||
|
||||
while(1){
|
||||
if (Serial3.available()){
|
||||
digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
|
||||
Serial1.write(Serial3.read());
|
||||
digitalWrite(B_LED_PIN, LOW);
|
||||
}
|
||||
if (Serial1.available()){
|
||||
digitalWrite(C_LED_PIN, HIGH); // Blink Red LED if we are receiving data from GPS
|
||||
Serial3.write(Serial1.read());
|
||||
digitalWrite(C_LED_PIN, LOW);
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
while(1){
|
||||
if (Serial1.available()){
|
||||
digitalWrite(B_LED_PIN, HIGH); // Blink Yellow LED if we are sending data to GPS
|
||||
Serial.write(Serial1.read());
|
||||
digitalWrite(B_LED_PIN, LOW);
|
||||
}
|
||||
if(Serial.available() > 0){
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -893,6 +894,11 @@ test_mag(uint8_t argc, const Menu::arg *argv)
|
||||
static int8_t
|
||||
test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
if(g.sonar_enabled == false){
|
||||
Serial.printf_P(PSTR("Sonar disabled\n"));
|
||||
return (0);
|
||||
}
|
||||
|
||||
print_hit_enter();
|
||||
while(1) {
|
||||
delay(100);
|
||||
@ -903,6 +909,7 @@ test_sonar(uint8_t argc, const Menu::arg *argv)
|
||||
return (0);
|
||||
}
|
||||
}
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
@ -910,7 +917,7 @@ static int8_t
|
||||
test_mission(uint8_t argc, const Menu::arg *argv)
|
||||
{
|
||||
//write out a basic mission to the EEPROM
|
||||
Location t;
|
||||
|
||||
/*{
|
||||
uint8_t id; ///< command id
|
||||
uint8_t options; ///< options bitmask (1<<0 = relative altitude)
|
||||
@ -919,7 +926,6 @@ test_mission(uint8_t argc, const Menu::arg *argv)
|
||||
int32_t lat; ///< param 3 - Lattitude * 10**7
|
||||
int32_t lng; ///< param 4 - Longitude * 10**7
|
||||
}*/
|
||||
byte alt_rel = 1;
|
||||
|
||||
// clear home
|
||||
{Location t = {0, 0, 0, 0, 0, 0};
|
||||
@ -963,7 +969,7 @@ test_mission(uint8_t argc, const Menu::arg *argv)
|
||||
g.waypoint_radius.set_and_save(3);
|
||||
|
||||
test_wp(NULL, NULL);
|
||||
|
||||
return (0);
|
||||
}
|
||||
|
||||
void print_hit_enter()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user