diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 809fed9c93..51cd79143c 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -189,10 +189,8 @@ const char* flight_mode_strings[] = { "FBW", "AUTO", "GCS_AUTO", - "POS_HOLD", - "RTL", - "TAKEOFF", - "LAND"}; + "LOITER", + "RTL"}; /* Radio values Channel assignments @@ -304,6 +302,9 @@ boolean land_complete; int landing_distance; // meters; long old_alt; // used for managing altitude rates int velocity_land; +bool nav_yaw_towards_wp; // point at the next WP + + // Loiter management // ----------------- @@ -363,7 +364,7 @@ struct Location next_WP; // next waypoint struct Location tell_command; // command for telemetry struct Location next_command; // command preloaded long target_altitude; // used for -long offset_altitude; // used for +//long offset_altitude; // used for boolean home_is_set; // Flag for if we have g_gps lock and have set the home location @@ -415,6 +416,8 @@ float load; // % MCU cycles used byte counter_one_herz; +byte GPS_failure_counter = 255; + //////////////////////////////////////////////////////////////////////////////// // Top-level logic //////////////////////////////////////////////////////////////////////////////// @@ -506,7 +509,10 @@ void medium_loop() //------------------------------- case 0: medium_loopCounter++; - update_GPS(); + + if(GPS_failure_counter > 0){ + update_GPS(); + } //readCommands(); if(g.compass_enabled){ @@ -643,7 +649,7 @@ void slow_loop() if(superslow_loopCounter >= 200){ // Execute every minute #if HIL_MODE != HIL_MODE_ATTITUDE - if(g.compass_enabled) { + if(g.compass_enabled){ compass.save_offsets(); } #endif @@ -678,11 +684,8 @@ void slow_loop() // between 1 and 5 Hz #else gcs.send_message(MSG_LOCATION); - // XXX - // gcs.send_message(MSG_CPU_LOAD, load*100); #endif - gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz break; @@ -693,10 +696,15 @@ void slow_loop() } } +// 1Hz loop void super_slow_loop() { if (g.log_bitmask & MASK_LOG_CUR) Log_Write_Current(); + + gcs.send_message(MSG_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz + // gcs.send_message(MSG_CPU_LOAD, load*100); + } void update_GPS(void) @@ -705,6 +713,8 @@ void update_GPS(void) update_GPS_light(); if (g_gps->new_data && g_gps->fix) { + GPS_failure_counter = 255; + // XXX We should be sending GPS data off one of the regular loops so that we send // no-GPS-fix data too #if GCS_PROTOCOL != GCS_PROTOCOL_MAVLINK @@ -746,6 +756,9 @@ void update_GPS(void) current_loc.lng = g_gps->longitude; // Lon * 10 * *7 current_loc.lat = g_gps->latitude; // Lat * 10 * *7 + }else{ + if(GPS_failure_counter > 0) + GPS_failure_counter--; } } @@ -928,7 +941,7 @@ void update_current_flight_mode(void) output_stabilize_pitch(); break; - case POSITION_HOLD: + case LOITER: // Yaw control // ----------- @@ -1047,7 +1060,10 @@ void update_alt() // altitude smoothing // ------------------ - calc_altitude_error(); + calc_altitude_smoothing_error(); + + + //calc_altitude_error(); // Amount of throttle to apply for hovering // ---------------------------------------- diff --git a/ArduCopterMega/Attitude.pde b/ArduCopterMega/Attitude.pde index 217d725fa3..f22ba0ed35 100644 --- a/ArduCopterMega/Attitude.pde +++ b/ArduCopterMega/Attitude.pde @@ -231,7 +231,7 @@ void calc_nav_throttle() float scaler = 1.0; if(error < 0){ - scaler = (altitude_sensor == BARO) ? .5 : .9; + scaler = (altitude_sensor == BARO) ? .5 : .8; } if(altitude_sensor == BARO){ @@ -276,7 +276,7 @@ void output_manual_yaw() void auto_yaw() { - if(next_WP.options & WP_OPT_YAW){ + if(nav_yaw_towards_wp){ nav_yaw = target_bearing; } output_yaw_with_hold(true); // hold yaw diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 021ad4d8e3..ce7252f06d 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -201,7 +201,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { case MAV_ACTION_LAUNCH: - set_mode(TAKEOFF); + //set_mode(TAKEOFF); break; case MAV_ACTION_RETURN: @@ -209,7 +209,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_ACTION_EMCY_LAND: - set_mode(LAND); + //set_mode(LAND); break; case MAV_ACTION_HALT: @@ -263,7 +263,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) case MAV_ACTION_REC_STOP: break; case MAV_ACTION_TAKEOFF: - set_mode(TAKEOFF); + //set_mode(TAKEOFF); break; case MAV_ACTION_NAVIGATE: @@ -271,7 +271,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; case MAV_ACTION_LAND: - set_mode(LAND); + //set_mode(LAND); break; case MAV_ACTION_LOITER: diff --git a/ArduCopterMega/Log.pde b/ArduCopterMega/Log.pde index 4f96c59a3e..5a0d5004fd 100644 --- a/ArduCopterMega/Log.pde +++ b/ArduCopterMega/Log.pde @@ -59,7 +59,7 @@ print_log_menu(void) // Pass it the capitalised name of the log option, as defined // in defines.h but without the LOG_ prefix. It will check for // the bit being set and print the name of the log option to suit. -#define PLOG(_s) if (g.log_bitmask & LOGBIT_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) + #define PLOG(_s) if (g.log_bitmask & MASK_LOG_ ## _s) Serial.printf_P(PSTR(" %S"), PSTR(#_s)) PLOG(ATTITUDE_FAST); PLOG(ATTITUDE_MED); PLOG(GPS); @@ -69,8 +69,8 @@ print_log_menu(void) PLOG(MODE); PLOG(RAW); PLOG(CMD); - PLOG(CURRENT); -#undef PLOG + PLOG(CUR); + #undef PLOG } Serial.println(); @@ -107,7 +107,10 @@ dump_log(uint8_t argc, const Menu::arg *argv) get_log_boundaries(last_log_num, dump_log, dump_log_start, dump_log_end); Serial.printf_P(PSTR("Dumping Log number %d, start page %d, end page %d\n"), - dump_log, dump_log_start, dump_log_end); + dump_log, + dump_log_start, + dump_log_end); + Log_Read(dump_log_start, dump_log_end); Serial.printf_P(PSTR("Log read complete\n")); } @@ -128,6 +131,7 @@ erase_logs(uint8_t argc, const Menu::arg *argv) DataFlash.WriteByte(LOG_INDEX_MSG); DataFlash.WriteByte(0); DataFlash.WriteByte(END_BYTE); + DataFlash.FinishWrite(); Serial.printf_P(PSTR("\nLog erased.\n")); } @@ -151,8 +155,8 @@ select_logs(uint8_t argc, const Menu::arg *argv) // if (!strcasecmp_P(argv[1].str, PSTR("all"))) { bits = ~(bits = 0); - }else{ -#define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= LOGBIT_ ## _s + } else { + #define TARG(_s) if (!strcasecmp_P(argv[1].str, PSTR(#_s))) bits |= MASK_LOG_ ## _s TARG(ATTITUDE_FAST); TARG(ATTITUDE_MED); TARG(GPS); @@ -162,8 +166,8 @@ select_logs(uint8_t argc, const Menu::arg *argv) TARG(MODE); TARG(RAW); TARG(CMD); - TARG(CURRENT); -#undef TARG + TARG(CUR); + #undef TARG } if (!strcasecmp_P(argv[0].str, PSTR("enable"))) { @@ -373,7 +377,7 @@ void Log_Write_Startup(byte type) struct Location cmd = get_wp_with_index(0); Log_Write_Cmd(0, &cmd); - for (int i=1; i7) logvar = DataFlash.ReadInt(); - else logvar = DataFlash.ReadByte(); - //if(y > 7) logvar = logvar/1000.f; + for (int y = 1; y < 9; y++) { + if(y < 3 || y > 7){ + logvar = DataFlash.ReadInt(); + }else{ + logvar = DataFlash.ReadByte(); + } Serial.print(logvar); Serial.print(comma); } @@ -556,14 +555,14 @@ void Log_Read_Cmd() long logvarl; Serial.print("CMD:"); - for(int i=1;i<4;i++) { + for(int i = 1; i < 4; i++) { logvarb = DataFlash.ReadByte(); - Serial.print(logvarb,DEC); + Serial.print(logvarb, DEC); Serial.print(comma); } - for(int i=1;i<4;i++) { + for(int i = 1; i < 4; i++) { logvarl = DataFlash.ReadLong(); - Serial.print(logvarl,DEC); + Serial.print(logvarl, DEC); Serial.print(comma); } Serial.println(" "); @@ -572,34 +571,24 @@ void Log_Read_Cmd() void Log_Read_Startup() { byte logbyte = DataFlash.ReadByte(); + if (logbyte == TYPE_AIRSTART_MSG) - Serial.print("AIR START - "); + Serial.printf_P(PSTR("AIR START - ")); else if (logbyte == TYPE_GROUNDSTART_MSG) - Serial.print("GROUND START - "); + Serial.printf_P(PSTR("GROUND START - ")); else - Serial.print("UNKNOWN STARTUP TYPE -"); - Serial.print(DataFlash.ReadByte(), DEC); - Serial.println(" commands in memory"); + 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() { - int log_roll; - int log_pitch; - uint16_t log_yaw; - - log_roll = DataFlash.ReadInt(); - log_pitch = DataFlash.ReadInt(); - log_yaw = (uint16_t)DataFlash.ReadInt(); - - Serial.print("ATT:"); - Serial.print(log_roll); - Serial.print(comma); - Serial.print(log_pitch); - Serial.print(comma); - Serial.print(log_yaw); - Serial.println(); + Serial.printf_P(PSTR("ATT: %d, %d, %d\n"), + DataFlash.ReadInt(), + DataFlash.ReadInt(), + (uint16_t)DataFlash.ReadInt()); } // Read a mode packet @@ -612,25 +601,16 @@ void Log_Read_Mode() // Read a GPS packet void Log_Read_GPS() { - - Serial.print("GPS:"); - Serial.print(DataFlash.ReadLong()); // Time - Serial.print(comma); - Serial.print((int)DataFlash.ReadByte()); // Fix - Serial.print(comma); - Serial.print((int)DataFlash.ReadByte()); // Num of Sats - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/t7, 7); // Lattitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/t7, 7); // Longitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // Baro/gps altitude mix - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // GPS altitude - Serial.print(comma); - Serial.print((float)DataFlash.ReadLong()/100.0); // Ground Speed - Serial.print(comma); - Serial.println((float)DataFlash.ReadLong()/100.0); // Ground Course + Serial.printf_P(PSTR("GPS: %ld, %d, %d, %4.7f, %4.7f, %4.4f, %4.4f, %4.4f, %4.4f\n"), + DataFlash.ReadLong(), + (int)DataFlash.ReadByte(), + (int)DataFlash.ReadByte(), + (float)DataFlash.ReadLong() / t7, + (float)DataFlash.ReadLong() / t7, + (float)DataFlash.ReadLong() / 100.0, + (float)DataFlash.ReadLong() / 100.0, + (float)DataFlash.ReadLong() / 100.0, + (float)DataFlash.ReadLong() / 100.0); } @@ -639,8 +619,8 @@ void Log_Read_Raw() { float logvar; Serial.print("RAW:"); - for (int y=0;y<6;y++) { - logvar = (float)DataFlash.ReadLong()/t7; + for (int y = 0; y < 6; y++) { + logvar = (float)DataFlash.ReadLong() / t7; Serial.print(logvar); Serial.print(comma); } @@ -651,61 +631,60 @@ void Log_Read_Raw() void Log_Read(int start_page, int end_page) { byte data; - byte log_step=0; - int packet_count=0; + byte log_step; + int packet_count; int page = start_page; DataFlash.StartRead(start_page); - while (page < end_page && page != -1) - { + while (page < end_page && page != -1){ data = DataFlash.ReadByte(); - switch(log_step) //This is a state machine to read the packets + switch(log_step) // This is a state machine to read the packets { case 0: - if(data==HEAD_BYTE1) // Head byte 1 + if(data == HEAD_BYTE1) // Head byte 1 log_step++; break; case 1: - if(data==HEAD_BYTE2) // Head byte 2 + if(data == HEAD_BYTE2) // Head byte 2 log_step++; else log_step = 0; break; case 2: - if(data==LOG_ATTITUDE_MSG){ + if(data == LOG_ATTITUDE_MSG){ Log_Read_Attitude(); log_step++; - }else if(data==LOG_MODE_MSG){ + }else if(data == LOG_MODE_MSG){ Log_Read_Mode(); log_step++; - }else if(data==LOG_CONTROL_TUNING_MSG){ + }else if(data == LOG_CONTROL_TUNING_MSG){ Log_Read_Control_Tuning(); log_step++; - }else if(data==LOG_NAV_TUNING_MSG){ + }else if(data == LOG_NAV_TUNING_MSG){ Log_Read_Nav_Tuning(); log_step++; - }else if(data==LOG_PERFORMANCE_MSG){ + }else if(data == LOG_PERFORMANCE_MSG){ Log_Read_Performance(); log_step++; - }else if(data==LOG_RAW_MSG){ + }else if(data == LOG_RAW_MSG){ Log_Read_Raw(); log_step++; - }else if(data==LOG_CMD_MSG){ + }else if(data == LOG_CMD_MSG){ Log_Read_Cmd(); log_step++; - }else if(data==LOG_CURRENT_MSG){ + }else if(data == LOG_CURRENT_MSG){ Log_Read_Current(); log_step++; - }else if(data==LOG_STARTUP_MSG){ + }else if(data == LOG_STARTUP_MSG){ Log_Read_Startup(); log_step++; }else { @@ -713,26 +692,23 @@ void Log_Read(int start_page, int end_page) Log_Read_GPS(); log_step++; }else{ - Serial.print("Error Reading Packet: "); - Serial.print(packet_count); + Serial.printf_P(PSTR("Error Reading Packet: %d\n"),packet_count); log_step = 0; // Restart, we have a problem... } } break; case 3: - if(data==END_BYTE){ + if(data == END_BYTE){ packet_count++; }else{ - Serial.print("Error Reading END_BYTE "); - Serial.println(data,DEC); + Serial.printf_P(PSTR("Error Reading END_BYTE: %d\n"),data); } - log_step=0; // Restart sequence: new packet... + log_step = 0; // Restart sequence: new packet... break; } page = DataFlash.GetPage(); } - Serial.print("Number of packets read: "); - Serial.println(packet_count); + Serial.printf_P(PSTR("Number of packets read: %d\n"), packet_count); } diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index d00aa39dc8..889bcb4240 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -10,257 +10,257 @@ byte mavdelay = 0; static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) { - if (sysid != mavlink_system.sysid) - { + if (sysid != mavlink_system.sysid){ return 1; - } - else if (compid != mavlink_system.compid) - { + + }else if(compid != mavlink_system.compid){ gcs.send_text(SEVERITY_LOW,"component id mismatch"); return 0; // XXX currently not receiving correct compid from gcs + + } else { + return 0; // no error } - else return 0; // no error } void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops) { uint64_t timeStamp = micros(); - switch(id) { + switch(id) { - case MSG_HEARTBEAT: - mavlink_msg_heartbeat_send( - chan, - system_type, - MAV_AUTOPILOT_ARDUPILOTMEGA); - break; + case MSG_HEARTBEAT: + mavlink_msg_heartbeat_send( + chan, + system_type, + MAV_AUTOPILOT_ARDUPILOTMEGA); + break; - case MSG_EXTENDED_STATUS: - { - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; + case MSG_EXTENDED_STATUS: + { + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; - switch(control_mode) { - case ACRO: - mode = MAV_MODE_MANUAL; - break; - case STABILIZE: - mode = MAV_MODE_GUIDED; - break; - case FBW: - mode = MAV_MODE_TEST1; - break; - case ALT_HOLD: - mode = MAV_MODE_TEST2; - break; - case POSITION_HOLD: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_HOLD; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case TAKEOFF: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LIFTOFF; - break; - case LAND: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LANDING; + switch(control_mode) { + case ACRO: + mode = MAV_MODE_MANUAL; + break; + case STABILIZE: + mode = MAV_MODE_GUIDED; + break; + case FBW: + mode = MAV_MODE_TEST1; + break; + case ALT_HOLD: + mode = MAV_MODE_TEST2; + break; + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_HOLD; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + //case TAKEOFF: + // mode = MAV_MODE_AUTO; + // nav_mode = MAV_NAV_LIFTOFF; + // break; + //case LAND: + // mode = MAV_MODE_AUTO; + // nav_mode = MAV_NAV_LANDING; + // break; + } + uint8_t status = MAV_STATE_ACTIVE; + uint8_t motor_block = false; + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + load * 1000, + battery_voltage1 * 1000, + motor_block, + packet_drops); break; } - uint8_t status = MAV_STATE_ACTIVE; - uint8_t motor_block = false; - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - load * 1000, - battery_voltage1 * 1000, - motor_block, - packet_drops); - break; - } + case MSG_ATTITUDE: + { + Vector3f omega = dcm.get_gyro(); + mavlink_msg_attitude_send( + chan, + timeStamp, + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); + break; + } - case MSG_ATTITUDE: - { - Vector3f omega = dcm.get_gyro(); - mavlink_msg_attitude_send( - chan, - timeStamp, - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); - break; + case MSG_LOCATION: + { + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed / 1.0e2 * rot.a.x, + g_gps->ground_speed / 1.0e2 * rot.b.x, + g_gps->ground_speed / 1.0e2 * rot.c.x); + break; + } + + case MSG_LOCAL_LOCATION: + { + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_local_position_send( + chan, + timeStamp, + ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, + ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), + (current_loc.alt - home.alt) / 1.0e2, + g_gps->ground_speed / 1.0e2 * rot.a.x, + g_gps->ground_speed / 1.0e2 * rot.b.x, + g_gps->ground_speed / 1.0e2 * rot.c.x); + break; + } + + case MSG_GPS_RAW: + { + mavlink_msg_gps_raw_send( + chan, + timeStamp, + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); + break; + } + + case MSG_SERVO_OUT: + { + uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + mavlink_msg_rc_channels_scaled_send( + chan, + g.rc_1.norm_output(), + g.rc_2.norm_output(), + g.rc_3.norm_output(), + g.rc_4.norm_output(), + 0,0,0,0,rssi); + break; + } + + case MSG_RADIO_IN: + { + uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.rc_1.radio_in, + g.rc_2.radio_in, + g.rc_3.radio_in, + g.rc_4.radio_in, + g.rc_5.radio_in, + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); + break; + } + + case MSG_RADIO_OUT: + { + mavlink_msg_servo_output_raw_send( + chan, + motor_out[0], + motor_out[1], + motor_out[2], + motor_out[3], + 0, 0, 0, 0); + break; + } + + case MSG_VFR_HUD: + { + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + dcm.yaw_sensor, + current_loc.alt / 100.0, + climb_rate, + nav_throttle); + break; + } + + #if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU: + { + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); + //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); + mavlink_msg_raw_imu_send( + chan, + timeStamp, + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); + + mavlink_msg_raw_pressure_send( + chan, + timeStamp, + adc.Ch(AIRSPEED_CH), + barometer.RawPress, + 0, + 0); + break; + } + #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE + + case MSG_GPS_STATUS: + { + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); + break; + } + + case MSG_CURRENT_WAYPOINT: + { + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); + break; + } + + defualt: + break; } - - case MSG_LOCATION: - { - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_global_position_int_send( - chan, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); - break; - } - - case MSG_LOCAL_LOCATION: - { - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_local_position_send( - chan, - timeStamp, - ToRad((current_loc.lat - home.lat) / 1.0e7) * radius_of_earth, - ToRad((current_loc.lng - home.lng) / 1.0e7) * radius_of_earth * cos(ToRad(home.lat / 1.0e7)), - (current_loc.alt - home.alt) / 1.0e2, - g_gps->ground_speed / 1.0e2 * rot.a.x, - g_gps->ground_speed / 1.0e2 * rot.b.x, - g_gps->ground_speed / 1.0e2 * rot.c.x); - break; - } - - case MSG_GPS_RAW: - { - mavlink_msg_gps_raw_send( - chan, - timeStamp, - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); - break; - } - - case MSG_SERVO_OUT: - { - uint8_t rssi = 1; - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with HIL maintainers - mavlink_msg_rc_channels_scaled_send( - chan, - g.rc_1.norm_output(), - g.rc_2.norm_output(), - g.rc_3.norm_output(), - g.rc_4.norm_output(), - 0,0,0,0,rssi); - break; - } - - case MSG_RADIO_IN: - { - uint8_t rssi = 1; - mavlink_msg_rc_channels_raw_send( - chan, - g.rc_1.radio_in, - g.rc_2.radio_in, - g.rc_3.radio_in, - g.rc_4.radio_in, - g.rc_5.radio_in, - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); - break; - } - - case MSG_RADIO_OUT: - { - mavlink_msg_servo_output_raw_send( - chan, - motor_out[0], - motor_out[1], - motor_out[2], - motor_out[3], - 0, 0, 0, 0); - break; - } - - case MSG_VFR_HUD: - { - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - dcm.yaw_sensor, - current_loc.alt / 100.0, - climb_rate, - nav_throttle); - break; - } - - #if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU: - { - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); - //Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z); - //Serial.printf_P(PSTR("sending gyro: %f %f %f\n"), gyro.x, gyro.y, gyro.z); - mavlink_msg_raw_imu_send( - chan, - timeStamp, - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); - - mavlink_msg_raw_pressure_send( - chan, - timeStamp, - adc.Ch(AIRSPEED_CH), - barometer.RawPress, - 0, - 0); - break; - } - #endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE - - case MSG_GPS_STATUS: - { - mavlink_msg_gps_status_send( - chan, - g_gps->num_sats, - NULL, - NULL, - NULL, - NULL, - NULL); - break; - } - - case MSG_CURRENT_WAYPOINT: - { - mavlink_msg_waypoint_current_send( - chan, - g.waypoint_index); - break; - } - - defualt: - break; - } } void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str) diff --git a/ArduCopterMega/commands.pde b/ArduCopterMega/commands.pde index 5729d5f2a3..9dabf855d2 100644 --- a/ArduCopterMega/commands.pde +++ b/ArduCopterMega/commands.pde @@ -40,9 +40,6 @@ struct Location get_wp_with_index(int i) mem = (WP_START_BYTE) + (i * WP_SIZE); temp.id = eeprom_read_byte((uint8_t*)mem); - mem++; - temp.options = eeprom_read_byte((uint8_t*)mem); - mem++; temp.p1 = eeprom_read_byte((uint8_t*)mem); @@ -57,8 +54,10 @@ struct Location get_wp_with_index(int i) } // Add on home altitude if we are a nav command - if(temp.options & WP_OPT_ALT_RELATIVE){ - temp.alt += home.alt; + if(temp.id < 0x70){ + //if((flight_options_mask & WP_OPT_ALT_RELATIVE) == 0){ + temp.alt += home.alt; + //} } return temp; @@ -77,7 +76,7 @@ void set_wp_with_index(struct Location temp, int i) //} // Store the location relatove to home - if(temp.options & WP_OPT_ALT_RELATIVE){ + if((flight_options_mask & WP_OPT_ALT_RELATIVE) == 0){ temp.alt -= home.alt; } @@ -86,9 +85,6 @@ void set_wp_with_index(struct Location temp, int i) eeprom_write_byte((uint8_t *) mem, temp.id); - mem++; - eeprom_write_byte((uint8_t *) mem, temp.options); - mem++; eeprom_write_byte((uint8_t *) mem, temp.p1); @@ -167,12 +163,6 @@ void set_next_WP(struct Location *wp) // ----------------------------------------------- target_altitude = current_loc.alt; - // XXX YUCK! - if(prev_WP.id != MAV_CMD_NAV_TAKEOFF && prev_WP.alt != home.alt && (next_WP.id == MAV_CMD_NAV_WAYPOINT || next_WP.id == MAV_CMD_NAV_LAND)) - offset_altitude = next_WP.alt - prev_WP.alt; - else - offset_altitude = 0; - // zero out our loiter vals to watch for missed waypoints loiter_delta = 0; loiter_sum = 0; diff --git a/ArduCopterMega/commands_logic.pde b/ArduCopterMega/commands_logic.pde index 899f549246..0980314482 100644 --- a/ArduCopterMega/commands_logic.pde +++ b/ArduCopterMega/commands_logic.pde @@ -81,9 +81,6 @@ handle_no_commands() return; switch (control_mode){ - case LAND: - // don't get a new command - break; //case GCS_AUTO: // set_mode(LOITER); diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 1df632d579..ff20eed224 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -88,11 +88,9 @@ #define FBW 3 // AUTO control #define AUTO 4 // AUTO control #define GCS_AUTO 5 // AUTO control -#define POSITION_HOLD 6 // Hold a single location +#define LOITER 6 // Hold a single location #define RTL 7 // AUTO control -#define TAKEOFF 8 // controlled decent rate -#define LAND 9 // controlled decent rate -#define NUM_MODES 10 +#define NUM_MODES 8 #define WP_OPT_ALT_RELATIVE (1<<0) @@ -207,18 +205,6 @@ #define MASK_LOG_CMD (1<<8) #define MASK_LOG_CUR (1<<9) -// bits in log_bitmask -#define LOGBIT_ATTITUDE_FAST (1<<0) -#define LOGBIT_ATTITUDE_MED (1<<1) -#define LOGBIT_GPS (1<<2) -#define LOGBIT_PM (1<<3) -#define LOGBIT_CTUN (1<<4) -#define LOGBIT_NTUN (1<<5) -#define LOGBIT_MODE (1<<6) -#define LOGBIT_RAW (1<<7) -#define LOGBIT_CMD (1<<8) -#define LOGBIT_CURRENT (1<<9) - // Waypoint Modes // ---------------- #define ABS_WP 0 diff --git a/ArduCopterMega/navigation.pde b/ArduCopterMega/navigation.pde index 130bb79271..396da343f0 100644 --- a/ArduCopterMega/navigation.pde +++ b/ArduCopterMega/navigation.pde @@ -100,20 +100,19 @@ void calc_bearing_error() void calc_altitude_error() { - //if(control_mode == AUTO && offset_altitude != 0) { - if(next_WP.options & WP_OPT_ALT_CHANGE || offset_altitude == 0) { - target_altitude = next_WP.alt; + altitude_error = next_WP.alt - current_loc.alt; +} +void calc_altitude_smoothing_error() +{ + // limit climb rates - we draw a straight line between first location and edge of waypoint_radius + target_altitude = next_WP.alt - ((wp_distance * (next_WP.alt - prev_WP.alt)) / (wp_totalDistance - g.waypoint_radius)); + + // stay within a certain range + if(prev_WP.alt > next_WP.alt){ + target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt); }else{ - // limit climb rates - we draw a straight line between first location and edge of waypoint_radius - target_altitude = next_WP.alt - ((wp_distance * offset_altitude) / (wp_totalDistance - g.waypoint_radius)); - - // stay within a certain range - if(prev_WP.alt > next_WP.alt){ - target_altitude = constrain(target_altitude, next_WP.alt, prev_WP.alt); - }else{ - target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); - } + target_altitude = constrain(target_altitude, prev_WP.alt, next_WP.alt); } altitude_error = target_altitude - current_loc.alt; diff --git a/ArduCopterMega/setup.pde b/ArduCopterMega/setup.pde index 22fa7e07c0..e3a4c45fad 100644 --- a/ArduCopterMega/setup.pde +++ b/ArduCopterMega/setup.pde @@ -647,7 +647,8 @@ void default_logs() { // convenience macro for testing LOG_* and setting LOGBIT_* - #define LOGBIT(_s) (LOG_ ## _s ? LOGBIT_ ## _s : 0) + #define LOGBIT(_s) (MASK_LOG_##_s ? MASK_LOG_##_s : 0) + g.log_bitmask = LOGBIT(ATTITUDE_FAST) | LOGBIT(ATTITUDE_MED) | @@ -658,7 +659,7 @@ void default_logs() LOGBIT(MODE) | LOGBIT(RAW) | LOGBIT(CMD) | - LOGBIT(CURRENT); + LOGBIT(CUR); #undef LOGBIT g.log_bitmask.save(); diff --git a/ArduCopterMega/system.pde b/ArduCopterMega/system.pde index ed293c63d4..9230c5c9f4 100644 --- a/ArduCopterMega/system.pde +++ b/ArduCopterMega/system.pde @@ -287,7 +287,7 @@ void set_mode(byte mode) update_auto(); break; - case POSITION_HOLD: + case LOITER: do_hold_position(); break; @@ -295,12 +295,6 @@ void set_mode(byte mode) do_RTL(); break; - case TAKEOFF: - break; - - case LAND: - break; - default: break; }