Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

lots of minor changes, Logs should be working better now. 

Pos hold is now called "Loiter" - I don't want to have a flight mode called HOLD POS...



git-svn-id: https://arducopter.googlecode.com/svn/trunk@1759 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-03-09 06:37:09 +00:00
parent 7f330fbb6c
commit 8114b717f2
11 changed files with 367 additions and 408 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
ArduCopterMega/ArduCopterMega.pde
View File

@ -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
// ----------------------------------------

4
ArduCopterMega/Attitude.pde
View File

@ -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

8
ArduCopterMega/GCS_Mavlink.pde
View File

@ -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:

180
ArduCopterMega/Log.pde
View File

@ -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; i<g.waypoint_total; i++){
for (int i = 1; i <= g.waypoint_total; i++){
cmd = get_wp_with_index(i);
Log_Write_Cmd(i, &cmd);
}
@ -486,7 +490,7 @@ void Log_Write_Current()
// Read a Current packet
void Log_Read_Current()
{
Serial.printf("CURR: %d, %4.4f, %4.4f, %d\n",
Serial.printf_P(PSTR("CURR: %d, %4.4f, %4.4f, %d\n"),
DataFlash.ReadInt(),
((float)DataFlash.ReadInt() / 100.f),
((float)DataFlash.ReadInt() / 100.f),
@ -498,11 +502,11 @@ void Log_Read_Control_Tuning()
{
float logvar;
Serial.print("CTUN:");
for (int y=1;y<10;y++) {
Serial.printf_P(PSTR("CTUN:"));
for (int y = 1; y < 10; y++) {
logvar = DataFlash.ReadInt();
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
if(y < 8) logvar = logvar/100.f;
if(y == 9) logvar = logvar/10000.f;
Serial.print(logvar);
Serial.print(comma);
}
@ -512,21 +516,14 @@ void Log_Read_Control_Tuning()
// Read a nav tuning packet
void Log_Read_Nav_Tuning()
{
Serial.print("NTUN:");
Serial.print((float)((uint16_t)DataFlash.ReadInt())/100.0); // Yaw from IMU
Serial.print(comma);
Serial.print(DataFlash.ReadInt()); // wp_distance
Serial.print(comma);
Serial.print((float)((uint16_t)DataFlash.ReadInt())/100.0); // target_bearing - Bearing to Target
Serial.print(comma);
Serial.print((float)((uint16_t)DataFlash.ReadInt())/100.0); // nav_bearing - Bearing to steer
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/100.0); // Altitude error
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/100.0); // Airspeed
Serial.print(comma);
Serial.print((float)DataFlash.ReadInt()/1000.0); // nav_gain_scaler
Serial.println(comma);
Serial.printf_P(PSTR("NTUN: %4.4f, %d, %4.4f, %4.4f, %4.4f, %4.4f, %4.4f,\n"),
(float)((uint16_t)DataFlash.ReadInt())/100.0,
DataFlash.ReadInt(),
(float)((uint16_t)DataFlash.ReadInt())/100.0,
(float)((uint16_t)DataFlash.ReadInt())/100.0,
(float)DataFlash.ReadInt()/100.0,
(float)DataFlash.ReadInt()/100.0,
(float)DataFlash.ReadInt()/1000.0);
}
// Read a performance packet
@ -539,10 +536,12 @@ void Log_Read_Performance()
pm_time = DataFlash.ReadLong();
Serial.print(pm_time);
Serial.print(comma);
for (int y=1;y<9;y++) {
if(y<3 || y>7) 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);
}

466
ArduCopterMega/Mavlink_Common.h
View File

@ -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)

20
ArduCopterMega/commands.pde
View File

@ -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;

3
ArduCopterMega/commands_logic.pde
View File

@ -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);

18
ArduCopterMega/defines.h
View File

@ -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

23
ArduCopterMega/navigation.pde
View File

@ -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;

5
ArduCopterMega/setup.pde
View File

@ -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();

8
ArduCopterMega/system.pde
View File

@ -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;
}