mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
Reworking Yaw some more. Yaw is testing really well with this version.
I have moved Pitch_Max to a different part of the code. It was limiting SIMPLE mode which was bad. Now it only limits in Auto modes, RTL, LOITER, etc. git-svn-id: https://arducopter.googlecode.com/svn/trunk@1899 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
parent
ffa33089b8
commit
87812c9eec
@ -4,13 +4,17 @@
|
||||
|
||||
// GPS is auto-selected
|
||||
|
||||
#define MAG_ORIENTATION AP_COMPASS_COMPONENTS_DOWN_PINS_BACK
|
||||
|
||||
#define GPS_PROTOCOL GPS_PROTOCOL_MTK
|
||||
|
||||
#define GCS_PROTOCOL GCS_PROTOCOL_NONE
|
||||
//#define GCS_PORT 0
|
||||
|
||||
#define SERIAL0_BAUD 38400
|
||||
|
||||
//# define STABILIZE_ROLL_P 0.4
|
||||
//# define STABILIZE_PITCH_P 0.4
|
||||
# define STABILIZE_ROLL_P 0.75
|
||||
# define STABILIZE_PITCH_P 0.75
|
||||
//# define STABILIZE_DAMPENER 0.1
|
||||
|
||||
|
||||
@ -23,4 +27,14 @@
|
||||
// Logging
|
||||
//#define LOG_CURRENT ENABLED
|
||||
|
||||
# define LOG_ATTITUDE_FAST DISABLED
|
||||
# define LOG_ATTITUDE_MED DISABLED
|
||||
# define LOG_GPS DISABLED
|
||||
# define LOG_PM DISABLED
|
||||
# define LOG_CTUN ENABLED
|
||||
# define LOG_NTUN DISABLED
|
||||
# define LOG_MODE DISABLED
|
||||
# define LOG_RAW DISABLED
|
||||
# define LOG_CMD DISABLED
|
||||
# define LOG_CURRENT DISABLED
|
||||
|
||||
|
@ -231,6 +231,7 @@ int max_stabilize_dampener; //
|
||||
int max_yaw_dampener; //
|
||||
boolean rate_yaw_flag; // used to transition yaw control from Rate control to Yaw hold
|
||||
byte yaw_debug;
|
||||
bool did_clear_yaw_control;
|
||||
|
||||
// LED output
|
||||
// ----------
|
||||
@ -583,6 +584,7 @@ void medium_loop()
|
||||
// we call these regardless of GPS because of the rapid nature of the yaw sensor
|
||||
// -----------------------------------------------------------------------------
|
||||
if(wp_distance < 800){ // 8 meters
|
||||
//if(g.rc_6.control_in > 500){ // 8 meters
|
||||
calc_loiter_nav();
|
||||
}else{
|
||||
calc_waypoint_nav();
|
||||
@ -795,6 +797,11 @@ void update_GPS(void)
|
||||
g_gps->update();
|
||||
update_GPS_light();
|
||||
|
||||
//current_loc.lng = 377697000; // Lon * 10 * *7
|
||||
//current_loc.lat = -1224318000; // Lat * 10 * *7
|
||||
//current_loc.alt = 100; // alt * 10 * *7
|
||||
//return;
|
||||
|
||||
if (g_gps->new_data && g_gps->fix) {
|
||||
|
||||
// XXX We should be sending GPS data off one of the regular loops so that we send
|
||||
@ -822,9 +829,6 @@ void update_GPS(void)
|
||||
}else{
|
||||
//Serial.printf("init Home!");
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CMD)
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
|
||||
// reset our nav loop timer
|
||||
//nav_loopTimer = millis();
|
||||
init_home();
|
||||
@ -980,14 +984,7 @@ void update_current_flight_mode(void)
|
||||
nav_pitch = 0;
|
||||
nav_roll = 0;
|
||||
|
||||
//if(g.rc_3.control_in)
|
||||
// get desired height from the throttle
|
||||
//next_WP.alt = home.alt + (g.rc_3.control_in); // 0 - 1000 (40 meters)
|
||||
//next_WP.alt = max(next_WP.alt, 30);
|
||||
|
||||
adjust_altitude();
|
||||
// !!! testing
|
||||
//next_WP.alt -= 500;
|
||||
|
||||
// Yaw control
|
||||
// -----------
|
||||
|
@ -13,6 +13,11 @@ init_pids()
|
||||
void
|
||||
control_nav_mixer()
|
||||
{
|
||||
// limit the nav pitch and roll of the copter
|
||||
long pmax = g.pitch_max.get();
|
||||
nav_roll = constrain(nav_roll, -pmax, pmax);
|
||||
nav_pitch = constrain(nav_pitch, -pmax, pmax);
|
||||
|
||||
// control +- 45° is mixed with the navigation request by the Autopilot
|
||||
// output is in degrees = target pitch and roll of copter
|
||||
g.rc_1.servo_out = g.rc_1.control_mix(nav_roll);
|
||||
@ -149,7 +154,9 @@ output_yaw_with_hold(boolean hold)
|
||||
if(g.rc_4.control_in == 0){
|
||||
// we are breaking;
|
||||
//g.rc_4.servo_out = (omega.z > 0) ? -600 : 600;
|
||||
g.rc_4.servo_out = (int)((float)g.rc_4.servo_out * (omega.z / 6.0));
|
||||
// adaptive braking
|
||||
g.rc_4.servo_out = (int)((1800.0 * omega.z) / 6.0);
|
||||
|
||||
yaw_debug = YAW_BRAKE;
|
||||
|
||||
}else{
|
||||
@ -160,8 +167,8 @@ output_yaw_with_hold(boolean hold)
|
||||
}
|
||||
|
||||
// Limit dampening to be equal to propotional term for symmetry
|
||||
//dampener = rate * g.hold_yaw_dampener; // 34377 * .175 = 6000
|
||||
//g.rc_4.servo_out -= constrain(dampener, -1800, 1800);
|
||||
dampener = rate * g.hold_yaw_dampener; // 34377 * .175 = 6000
|
||||
g.rc_4.servo_out -= constrain(dampener, -1800, 1800);
|
||||
|
||||
// Limit Output
|
||||
g.rc_4.servo_out = constrain(g.rc_4.servo_out, -1800, 1800); // limit to 24°
|
||||
@ -263,7 +270,11 @@ yaw control
|
||||
void output_manual_yaw()
|
||||
{
|
||||
if(g.rc_3.control_in == 0){
|
||||
clear_yaw_control();
|
||||
// we want to only call this once
|
||||
if(did_clear_yaw_control == false){
|
||||
clear_yaw_control();
|
||||
did_clear_yaw_control = true;
|
||||
}
|
||||
}else{
|
||||
// Yaw control
|
||||
if(g.rc_4.control_in == 0){
|
||||
@ -271,6 +282,8 @@ void output_manual_yaw()
|
||||
}else{
|
||||
output_yaw_with_hold(false); // rate control yaw
|
||||
}
|
||||
|
||||
did_clear_yaw_control = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -77,9 +77,8 @@ print_log_menu(void)
|
||||
if (last_log_num == 0) {
|
||||
Serial.printf_P(PSTR("\nNo logs\n"));
|
||||
}else{
|
||||
|
||||
Serial.printf_P(PSTR("\n%d logs\n"), last_log_num);
|
||||
for(int i=1;i<last_log_num+1;i++) {
|
||||
for(int i = 1; i < last_log_num + 1; i++) {
|
||||
get_log_boundaries(last_log_num, i, log_start, log_end);
|
||||
Serial.printf_P(PSTR("Log # %d, start %d, end %d\n"),
|
||||
i, log_start, log_end);
|
||||
@ -112,7 +111,7 @@ dump_log(uint8_t argc, const Menu::arg *argv)
|
||||
dump_log_end);
|
||||
|
||||
Log_Read(dump_log_start, dump_log_end);
|
||||
Serial.printf_P(PSTR("Complete\n"));
|
||||
Serial.printf_P(PSTR("Done\n"));
|
||||
}
|
||||
|
||||
static int8_t
|
||||
@ -227,31 +226,36 @@ void start_new_log(byte num_existing_logs)
|
||||
int end_pages[50] = {0,0,0};
|
||||
byte data;
|
||||
|
||||
if(num_existing_logs > 0) {
|
||||
if (num_existing_logs > 0) {
|
||||
for(int i=0;i<num_existing_logs;i++) {
|
||||
get_log_boundaries(num_existing_logs, i+1,start_pages[i],end_pages[i]);
|
||||
}
|
||||
end_pages[num_existing_logs - 1] = find_last_log_page(start_pages[num_existing_logs - 1]);
|
||||
}
|
||||
|
||||
if(end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
|
||||
if (end_pages[num_existing_logs - 1] < 4095 && num_existing_logs < MAX_NUM_LOGS) {
|
||||
if(num_existing_logs > 0)
|
||||
start_pages[num_existing_logs] = end_pages[num_existing_logs - 1] + 1;
|
||||
else
|
||||
start_pages[0] = 2;
|
||||
|
||||
num_existing_logs++;
|
||||
|
||||
DataFlash.StartWrite(1);
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_INDEX_MSG);
|
||||
DataFlash.WriteByte(num_existing_logs);
|
||||
|
||||
for(int i=0;i<MAX_NUM_LOGS;i++) {
|
||||
DataFlash.WriteInt(start_pages[i]);
|
||||
DataFlash.WriteInt(end_pages[i]);
|
||||
}
|
||||
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
DataFlash.FinishWrite();
|
||||
DataFlash.StartWrite(start_pages[num_existing_logs-1]);
|
||||
|
||||
}else{
|
||||
gcs.send_text_P(SEVERITY_LOW,PSTR("<start_new_log> Logs full"));
|
||||
}
|
||||
@ -364,26 +368,6 @@ void Log_Write_Cmd(byte num, struct Location *wp)
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
}
|
||||
|
||||
void Log_Write_Startup(byte type)
|
||||
{
|
||||
DataFlash.WriteByte(HEAD_BYTE1);
|
||||
DataFlash.WriteByte(HEAD_BYTE2);
|
||||
DataFlash.WriteByte(LOG_STARTUP_MSG);
|
||||
DataFlash.WriteByte(type);
|
||||
DataFlash.WriteByte(g.waypoint_total);
|
||||
DataFlash.WriteByte(END_BYTE);
|
||||
|
||||
// create a location struct to hold the temp Waypoints for printing
|
||||
struct Location cmd = get_wp_with_index(0);
|
||||
Log_Write_Cmd(0, &cmd);
|
||||
|
||||
for (int i = 1; i <= g.waypoint_total; i++){
|
||||
cmd = get_wp_with_index(i);
|
||||
Log_Write_Cmd(i, &cmd);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// Write a control tuning packet. Total length : 22 bytes
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
void Log_Write_Control_Tuning()
|
||||
@ -585,20 +569,6 @@ void Log_Read_Cmd()
|
||||
Serial.println(" ");
|
||||
}
|
||||
|
||||
void Log_Read_Startup()
|
||||
{
|
||||
byte logbyte = DataFlash.ReadByte();
|
||||
|
||||
if (logbyte == TYPE_AIRSTART_MSG)
|
||||
Serial.printf_P(PSTR("AIR START - "));
|
||||
else if (logbyte == TYPE_GROUNDSTART_MSG)
|
||||
Serial.printf_P(PSTR("GROUND START - "));
|
||||
else
|
||||
Serial.printf_P(PSTR("UNKNOWN STARTUP - "));
|
||||
|
||||
Serial.printf_P(PSTR(" %d commands in memory\n"),(int)DataFlash.ReadByte());
|
||||
}
|
||||
|
||||
// Read an attitude packet
|
||||
void Log_Read_Attitude()
|
||||
{
|
||||
@ -648,26 +618,30 @@ void Log_Read_Raw()
|
||||
void Log_Read(int start_page, int end_page)
|
||||
{
|
||||
byte data;
|
||||
byte log_step = 0;
|
||||
int packet_count = 0;
|
||||
int page = start_page;
|
||||
|
||||
byte log_step = 0;
|
||||
int packet_count = 0;
|
||||
int page = start_page;
|
||||
|
||||
DataFlash.StartRead(start_page);
|
||||
|
||||
while (page < end_page && page != -1){
|
||||
|
||||
data = DataFlash.ReadByte();
|
||||
switch(log_step) // This is a state machine to read the packets
|
||||
{
|
||||
|
||||
// This is a state machine to read the packets
|
||||
switch(log_step){
|
||||
case 0:
|
||||
if(data == HEAD_BYTE1) // Head byte 1
|
||||
log_step++;
|
||||
break;
|
||||
|
||||
case 1:
|
||||
if(data == HEAD_BYTE2) // Head byte 2
|
||||
log_step++;
|
||||
else
|
||||
log_step = 0;
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if(data == LOG_ATTITUDE_MSG){
|
||||
Log_Read_Attitude();
|
||||
@ -702,30 +676,32 @@ void Log_Read(int start_page, int end_page)
|
||||
log_step++;
|
||||
|
||||
}else if(data == LOG_STARTUP_MSG){
|
||||
Log_Read_Startup();
|
||||
// not implemented
|
||||
log_step++;
|
||||
}else {
|
||||
if(data == 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...
|
||||
}
|
||||
|
||||
}else if(data == LOG_GPS_MSG){
|
||||
Log_Read_GPS();
|
||||
log_step++;
|
||||
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Error P: %d\n"),packet_count);
|
||||
log_step = 0; // Restart, we have a problem...
|
||||
}
|
||||
break;
|
||||
|
||||
case 3:
|
||||
if(data == END_BYTE){
|
||||
packet_count++;
|
||||
}else{
|
||||
Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data);
|
||||
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);
|
||||
|
||||
page = DataFlash.GetPage();
|
||||
}
|
||||
//Serial.printf_P(PSTR("# of packets read: %d\n"), packet_count);
|
||||
}
|
||||
|
||||
|
||||
|
@ -255,6 +255,14 @@
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// Y6 Support
|
||||
|
||||
#ifndef Y6_MOTOR_SCALER
|
||||
# define Y6_MOTOR_SCALER 0.92
|
||||
#endif
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// ACRO Rate Control
|
||||
|
||||
@ -312,7 +320,7 @@
|
||||
# define STABILIZE_ROLL_I 0.1 //
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_D
|
||||
# define STABILIZE_ROLL_D 0.11
|
||||
# define STABILIZE_ROLL_D 0.13
|
||||
#endif
|
||||
#ifndef STABILIZE_ROLL_IMAX
|
||||
# define STABILIZE_ROLL_IMAX 10 // 10 degrees
|
||||
@ -325,13 +333,12 @@
|
||||
# define STABILIZE_PITCH_I 0.1
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_D
|
||||
# define STABILIZE_PITCH_D 0.11
|
||||
# define STABILIZE_PITCH_D 0.13
|
||||
#endif
|
||||
#ifndef STABILIZE_PITCH_IMAX
|
||||
# define STABILIZE_PITCH_IMAX 10
|
||||
#endif
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
// YAW Control
|
||||
//
|
||||
|
@ -59,7 +59,7 @@ set_servos_4()
|
||||
g.rc_3.servo_out = constrain(g.rc_3.servo_out, 0, 1000);
|
||||
|
||||
if(g.rc_3.servo_out > 0)
|
||||
out_min = g.rc_3.radio_min + 60;
|
||||
out_min = g.rc_3.radio_min + 90;
|
||||
|
||||
//Serial.printf("out: %d %d %d %d\t\t", g.rc_1.servo_out, g.rc_2.servo_out, g.rc_3.servo_out, g.rc_4.servo_out);
|
||||
|
||||
@ -71,7 +71,8 @@ set_servos_4()
|
||||
|
||||
// limit Yaw control so we don't clip and loose altitude
|
||||
// this is only a partial solution.
|
||||
//g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
|
||||
|
||||
// g.rc_4.pwm_out = min(g.rc_4.pwm_out, (g.rc_3.radio_out - out_min));
|
||||
|
||||
//Serial.printf("out: %d %d %d %d\n", g.rc_1.radio_out, g.rc_2.radio_out, g.rc_3.radio_out, g.rc_4.radio_out);
|
||||
//Serial.printf("yaw: %d ", g.rc_4.radio_out);
|
||||
@ -163,15 +164,15 @@ set_servos_4()
|
||||
int pitch_out = g.rc_2.pwm_out / 2;
|
||||
|
||||
//left
|
||||
motor_out[CH_2] = ((g.rc_3.radio_out * 0.92) + roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_2] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) + roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_3] = g.rc_3.radio_out + roll_out + pitch_out; // CW
|
||||
|
||||
//right
|
||||
motor_out[CH_7] = ((g.rc_3.radio_out * 0.92) - roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_7] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - roll_out + pitch_out); // CCW TOP
|
||||
motor_out[CH_1] = g.rc_3.radio_out - roll_out + pitch_out; // CW
|
||||
|
||||
//back
|
||||
motor_out[CH_8] = ((g.rc_3.radio_out * 0.92) - g.rc_2.pwm_out); // CCW TOP
|
||||
motor_out[CH_8] = ((g.rc_3.radio_out * Y6_MOTOR_SCALER) - g.rc_2.pwm_out); // CCW TOP
|
||||
motor_out[CH_4] = g.rc_3.radio_out - g.rc_2.pwm_out; // CW
|
||||
|
||||
//yaw
|
||||
@ -223,6 +224,13 @@ set_servos_4()
|
||||
//g.pid_baro_throttle.kD((float)g.rc_6.control_in / 1000); // 0 to 1
|
||||
//g.pid_baro_throttle.kP((float)g.rc_6.control_in / 4000); // 0 to .25
|
||||
|
||||
|
||||
// uncomment me out to try in flight dampening, 0 = unflyable, .2 = unfun, .13 worked for me.
|
||||
// use test,radio to get the value to use in your config.
|
||||
//g.stabilize_dampener.set((float)g.rc_6.control_in / 1000.0);
|
||||
|
||||
|
||||
|
||||
/*
|
||||
// ROLL and PITCH
|
||||
// make sure you init_pids() after changing the kP
|
||||
@ -239,16 +247,41 @@ set_servos_4()
|
||||
init_pids();
|
||||
//*/
|
||||
|
||||
/*
|
||||
Serial.printf("yaw: %d, lat_e: %ld, lng_e: %ld, \tnlat: %ld, nlng: %ld,\tnrll: %ld, nptc: %ld, \tcx: %.2f, sy: %.2f, \ttber: %ld, \tnber: %ld\n",
|
||||
(int)(dcm.yaw_sensor / 100),
|
||||
lat_error,
|
||||
long_error,
|
||||
nav_lat,
|
||||
nav_lon,
|
||||
nav_roll,
|
||||
nav_pitch,
|
||||
cos_yaw_x,
|
||||
sin_yaw_y,
|
||||
target_bearing,
|
||||
nav_bearing);
|
||||
//*/
|
||||
|
||||
/*
|
||||
|
||||
gcs_simple.write_byte(control_mode);
|
||||
//gcs_simple.write_int(motor_out[CH_1]);
|
||||
//gcs_simple.write_int(motor_out[CH_2]);
|
||||
//gcs_simple.write_int(motor_out[CH_3]);
|
||||
//gcs_simple.write_int(motor_out[CH_4]);
|
||||
|
||||
gcs_simple.write_int(g.rc_3.servo_out);
|
||||
|
||||
gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
|
||||
|
||||
gcs_simple.write_int((int)nav_lat);
|
||||
gcs_simple.write_int((int)nav_lon);
|
||||
gcs_simple.write_int((int)nav_roll);
|
||||
gcs_simple.write_int((int)nav_pitch);
|
||||
|
||||
//gcs_simple.write_int((int)(cos_yaw_x * 100));
|
||||
//gcs_simple.write_int((int)(sin_yaw_y * 100));
|
||||
|
||||
gcs_simple.write_long(current_loc.lat); //28
|
||||
gcs_simple.write_long(current_loc.lng); //32
|
||||
gcs_simple.write_int((int)current_loc.alt); //34
|
||||
@ -257,10 +290,9 @@ set_servos_4()
|
||||
gcs_simple.write_long(next_WP.lng);
|
||||
gcs_simple.write_int((int)next_WP.alt); //44
|
||||
|
||||
gcs_simple.write_int((int)(target_bearing / 100));
|
||||
gcs_simple.write_int((int)(nav_bearing / 100));
|
||||
gcs_simple.write_int((int)(nav_yaw / 100));
|
||||
gcs_simple.write_int((int)(dcm.yaw_sensor / 100));
|
||||
gcs_simple.write_int((int)(target_bearing / 100));
|
||||
gcs_simple.write_int((int)(nav_bearing / 100));
|
||||
gcs_simple.write_int((int)(nav_yaw / 100));
|
||||
|
||||
if(altitude_sensor == BARO){
|
||||
gcs_simple.write_int((int)g.pid_baro_throttle.get_integrator());
|
||||
@ -269,17 +301,15 @@ set_servos_4()
|
||||
}
|
||||
|
||||
gcs_simple.write_int(g.throttle_cruise);
|
||||
|
||||
|
||||
//gcs_simple.write_int((int)(cos_yaw_x * 100));
|
||||
//gcs_simple.write_int((int)(sin_yaw_y * 100));
|
||||
//gcs_simple.write_int((int)(nav_yaw / 100));
|
||||
|
||||
gcs_simple.write_int(g.throttle_cruise);
|
||||
|
||||
//24
|
||||
|
||||
gcs_simple.flush(10); // Message ID
|
||||
|
||||
//*/
|
||||
//Serial.printf("\n tb %d\n", (int)(target_bearing / 100));
|
||||
//Serial.printf("\n nb %d\n", (int)(nav_bearing / 100));
|
||||
//Serial.printf("\n dcm %d\n", (int)(dcm.yaw_sensor / 100));
|
||||
|
||||
/*Serial.printf("a %ld, e %ld, i %d, t %d, b %4.2f\n",
|
||||
current_loc.alt,
|
||||
|
@ -108,10 +108,6 @@ void calc_loiter_nav()
|
||||
//EAST -1000 * -1 + 1000 * 0 = 1000 // pitch back
|
||||
//SOUTH -1000 * 0 + 1000 * -1 = -1000 // pitch forward
|
||||
|
||||
long pmax = g.pitch_max.get();
|
||||
|
||||
nav_roll = constrain(nav_roll, -pmax, pmax);
|
||||
nav_pitch = constrain(nav_pitch, -pmax, pmax);
|
||||
}
|
||||
|
||||
void calc_waypoint_nav()
|
||||
@ -129,9 +125,6 @@ void calc_waypoint_nav()
|
||||
nav_roll = (float)nav_lat * cos_nav_x;
|
||||
nav_pitch = -(float)nav_lat * sin_nav_y;
|
||||
|
||||
long pmax = g.pitch_max.get();
|
||||
nav_roll = constrain(nav_roll, -pmax, pmax);
|
||||
nav_pitch = constrain(nav_pitch, -pmax, pmax);
|
||||
}
|
||||
|
||||
void calc_bearing_error()
|
||||
|
@ -41,10 +41,6 @@ MENU(main_menu, "ACM", main_menu_commands);
|
||||
|
||||
void init_ardupilot()
|
||||
{
|
||||
byte last_log_num;
|
||||
int last_log_start;
|
||||
int last_log_end;
|
||||
|
||||
// Console serial port
|
||||
//
|
||||
// The console port buffers are defined to be sufficiently large to support
|
||||
@ -67,9 +63,9 @@ void init_ardupilot()
|
||||
// XXX the 128 byte receive buffer may be too small for NMEA, depending
|
||||
// on the message set configured.
|
||||
//
|
||||
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
|
||||
#if GPS_PROTOCOL != GPS_PROTOCOL_IMU
|
||||
Serial1.begin(38400, 128, 16);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
// Telemetry port.
|
||||
//
|
||||
@ -81,8 +77,8 @@ void init_ardupilot()
|
||||
//
|
||||
Serial3.begin(SERIAL3_BAUD, 128, 128);
|
||||
|
||||
Serial.printf_P(PSTR("\n\nInit ArduCopterMega"
|
||||
"\n\nFree RAM: %lu\n"),
|
||||
Serial.printf_P(PSTR("\n\nInit ACM"
|
||||
"\n\nRAM: %lu\n"),
|
||||
freeRAM());
|
||||
|
||||
|
||||
@ -131,21 +127,22 @@ void init_ardupilot()
|
||||
// Serial.printf_P(PSTR("using %u bytes of memory\n"), AP_Var::get_memory_use());
|
||||
}
|
||||
|
||||
#ifdef RADIO_OVERRIDE_DEFAULTS
|
||||
|
||||
#ifdef RADIO_OVERRIDE_DEFAULTS
|
||||
{
|
||||
int16_t rc_override[8] = RADIO_OVERRIDE_DEFAULTS;
|
||||
APM_RC.setHIL(rc_override);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
init_rc_in(); // sets up rc channels from radio
|
||||
init_rc_out(); // sets up the timer libs
|
||||
init_camera();
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
adc.Init(); // APM ADC library initialization
|
||||
barometer.Init(); // APM Abs Pressure sensor initialization
|
||||
#endif
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
#endif
|
||||
|
||||
// Do GPS init
|
||||
//g_gps = &GPS;
|
||||
@ -159,18 +156,14 @@ void init_ardupilot()
|
||||
gcs.init(&Serial);
|
||||
#endif
|
||||
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_MODE)
|
||||
Log_Write_Mode(control_mode);
|
||||
|
||||
if(g.compass_enabled)
|
||||
init_compass();
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
if(g.sonar_enabled){
|
||||
sonar.init(SONAR_PIN, &adc);
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
pinMode(C_LED_PIN, OUTPUT); // GPS status LED
|
||||
pinMode(A_LED_PIN, OUTPUT); // GPS status LED
|
||||
@ -198,33 +191,30 @@ void init_ardupilot()
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
last_log_num = get_num_logs();
|
||||
start_new_log(last_log_num);
|
||||
}
|
||||
|
||||
// read in the flight switches
|
||||
update_servo_switches();
|
||||
|
||||
// read in the flight switches
|
||||
//update_servo_switches();
|
||||
|
||||
//imu.init_gyro(IMU::WARM_START);
|
||||
|
||||
// All of the Gyro calibrations
|
||||
// ----------------------------
|
||||
startup_ground();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CMD)
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS) {
|
||||
default_log_bitmask();
|
||||
}
|
||||
|
||||
// set the correct flight mode
|
||||
// ---------------------------
|
||||
reset_control_switch();
|
||||
|
||||
// Logging:
|
||||
// --------
|
||||
DataFlash.Init(); // DataFlash log initialization
|
||||
// setup the log bitmask
|
||||
if (g.log_bitmask & MASK_LOG_SET_DEFAULTS)
|
||||
default_log_bitmask();
|
||||
|
||||
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
|
||||
byte last_log_num = get_num_logs();
|
||||
start_new_log(last_log_num);
|
||||
}
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_MODE)
|
||||
Log_Write_Mode(control_mode);
|
||||
}
|
||||
|
||||
//********************************************************************************
|
||||
@ -257,14 +247,6 @@ void startup_ground(void)
|
||||
report_imu();
|
||||
#endif
|
||||
|
||||
// read the radio to set trims
|
||||
// ---------------------------
|
||||
//trim_radio();
|
||||
|
||||
if (g.log_bitmask & MASK_LOG_CMD)
|
||||
Log_Write_Startup(TYPE_GROUNDSTART_MSG);
|
||||
|
||||
|
||||
#if HIL_MODE != HIL_MODE_ATTITUDE
|
||||
// read Baro pressure at ground
|
||||
//-----------------------------
|
||||
|
Loading…
Reference in New Issue
Block a user