aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Camera stabilization, Mavlink updates

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@2238 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jasonshort 2011-05-10 00:00:05 +00:00
parent 8e55b054f9
commit 7d43ce5159
2 changed files with 80 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
ArduCopterMega/ArduCopterMega.pde
View File

@ -174,7 +174,6 @@ GPS *g_gps;
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// //
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
//GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3);
GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3); GCS_MAVLINK gcs(Parameters::k_param_streamrates_port3);
#else #else
// If we are not using a GCS, we need a stub that does nothing. // If we are not using a GCS, we need a stub that does nothing.
@ -430,6 +429,9 @@ unsigned long medium_loopTimer; // Time in miliseconds of navigation control
byte medium_loopCounter; // Counters for branching from main control loop to slower loops byte medium_loopCounter; // Counters for branching from main control loop to slower loops
uint8_t delta_ms_medium_loop; uint8_t delta_ms_medium_loop;
unsigned long fiftyhz_loopTimer;
uint8_t delta_ms_fiftyhz;
byte slow_loopCounter; byte slow_loopCounter;
int superslow_loopCounter; int superslow_loopCounter;
byte flight_timer; // for limiting the execution of flight mode thingys byte flight_timer; // for limiting the execution of flight mode thingys
@ -478,12 +480,13 @@ void loop()
fast_loopTimeStamp = millis(); fast_loopTimeStamp = millis();
} }
if (millis() - medium_loopTimer > 19) { if (millis() - fiftyhz_loopTimer > 19) {
delta_ms_medium_loop = millis() - medium_loopTimer; delta_ms_fiftyhz = millis() - fiftyhz_loopTimer;
medium_loopTimer = millis(); fiftyhz_loopTimer = millis();
medium_loop(); medium_loop();
fifty_hz_loop();
counter_one_herz++; counter_one_herz++;
if(counter_one_herz == 50){ if(counter_one_herz == 50){
super_slow_loop(); super_slow_loop();
@ -504,7 +507,7 @@ void loop()
} }
} }
// Main loop 50-100Hz // Main loop 160Hz
void fast_loop() void fast_loop()
{ {
// IMU DCM Algorithm // IMU DCM Algorithm
@ -527,10 +530,10 @@ void fast_loop()
// ------------------------------ // ------------------------------
throttle_integrator += g.rc_3.servo_out; throttle_integrator += g.rc_3.servo_out;
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK #if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
// HIL for a copter needs very fast update of the servo values // HIL for a copter needs very fast update of the servo values
gcs.send_message(MSG_RADIO_OUT); gcs.send_message(MSG_RADIO_OUT);
#endif #endif
} }
void medium_loop() void medium_loop()
@ -551,20 +554,20 @@ void medium_loop()
case 0: case 0:
medium_loopCounter++; medium_loopCounter++;
if(GPS_enabled){ if(GPS_enabled){
update_GPS(); update_GPS();
} }
//readCommands(); //readCommands();
if(g.compass_enabled){ #if HIL_MODE != HIL_MODE_ATTITUDE
compass.read(); // Read magnetometer if(g.compass_enabled){
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading compass.read(); // Read magnetometer
//compass.calculate(dcm.roll, dcm.pitch); // Calculate heading compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix()); //compass.calculate(dcm.roll, dcm.pitch); // Calculate heading
} compass.null_offsets(dcm.get_dcm_matrix());
}
#endif
break; break;
// This case performs some navigation computations // This case performs some navigation computations
@ -632,21 +635,19 @@ void medium_loop()
case 3: case 3:
medium_loopCounter++; medium_loopCounter++;
if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Attitude();
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if (g.log_bitmask & MASK_LOG_CTUN) if (g.log_bitmask & MASK_LOG_ATTITUDE_MED && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST))
Log_Write_Control_Tuning(); Log_Write_Attitude();
if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning();
#endif #endif
if (g.log_bitmask & MASK_LOG_NTUN) if (g.log_bitmask & MASK_LOG_NTUN)
Log_Write_Nav_Tuning(); Log_Write_Nav_Tuning();
if (g.log_bitmask & MASK_LOG_GPS){ if (g.log_bitmask & MASK_LOG_GPS){
//if(home_is_set){ Log_Write_GPS();
Log_Write_GPS();
//}
} }
// XXX this should be a "GCS medium loop" interface // XXX this should be a "GCS medium loop" interface
@ -657,12 +658,18 @@ void medium_loop()
#else #else
gcs.send_message(MSG_ATTITUDE); // Sends attitude data gcs.send_message(MSG_ATTITUDE); // Sends attitude data
#endif #endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(5,45);
#endif
break; break;
// This case controls the slow loop // This case controls the slow loop
//--------------------------------- //---------------------------------
case 4: case 4:
medium_loopCounter = 0; medium_loopCounter = 0;
delta_ms_medium_loop = millis() - medium_loopTimer;
medium_loopTimer = millis();
if (g.battery_monitoring != 0){ if (g.battery_monitoring != 0){
read_battery(); read_battery();
@ -687,39 +694,58 @@ void medium_loop()
break; break;
} }
// stuff that happens at 50 hz }
// ---------------------------
// stuff that happens at 50 hz
// ---------------------------
void fifty_hz_loop()
{
// use Yaw to find our bearing error // use Yaw to find our bearing error
calc_bearing_error(); calc_bearing_error();
// guess how close we are - fixed observer calc // guess how close we are - fixed observer calc
//calc_distance_error(); //calc_distance_error();
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) # if HIL_MODE == HIL_MODE_DISABLED
Log_Write_Attitude(); if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
Log_Write_Attitude();
#if HIL_MODE != HIL_MODE_ATTITUDE
if (g.log_bitmask & MASK_LOG_RAW) if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw(); Log_Write_Raw();
#endif #endif
#if GCS_PROTOCOL == 6 // This is here for Benjamin Pelletier. Please do not remove without checking with me. Doug W //#if ENABLE_CAM
readgcsinput(); camera_stabilization();
//#endif
// XXX is it appropriate to be doing the comms below on the fast loop?
#if HIL_MODE != HIL_MODE_DISABLED && HIL_PORT != GCS_PORT
// kick the HIL to process incoming sensor packets
hil.update();
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK
hil.data_stream_send(45,1000);
#else
hil.send_message(MSG_SERVO_OUT);
#endif
#elif HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && HIL_MODE == HIL_MODE_DISABLED && HIL_PORT == 0
// Case for hil object on port 0 just for mission planning
hil.update();
hil.data_stream_send(45,1000);
#endif #endif
#if ENABLE_CAM // kick the GCS to process uplink data
camera_stabilization(); gcs.update();
#endif
// kick the GCS to process uplink data
gcs.update();
#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK #if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK
gcs.data_stream_send(45,1000); gcs.data_stream_send(45,1000);
#endif #endif
// XXX this should be absorbed into the above,
// or be a "GCS fast loop" interface
} }
void slow_loop() void slow_loop()
{ {
// This is the slow (3 1/3 Hz) loop pieces // This is the slow (3 1/3 Hz) loop pieces
@ -768,6 +794,11 @@ void slow_loop()
// between 1 and 5 Hz // between 1 and 5 Hz
#else #else
gcs.send_message(MSG_LOCATION); gcs.send_message(MSG_LOCATION);
gcs.send_message(MSG_CPU_LOAD, load*100);
#endif
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.data_stream_send(1,5);
#endif #endif
#if CHANNEL_6_TUNING != CH6_NONE #if CHANNEL_6_TUNING != CH6_NONE
@ -795,7 +826,10 @@ void super_slow_loop()
Log_Write_Current(); 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_HEARTBEAT); // XXX This is running at 3 1/3 Hz instead of 1 Hz
// gcs.send_message(MSG_CPU_LOAD, load*100);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_HEARTBEAT);
#endif
//if(gcs_simple.read()){ //if(gcs_simple.read()){
// Serial.print("!"); // Serial.print("!");
@ -1147,6 +1181,11 @@ void read_AHRS(void)
{ {
// Perform IMU calculations and get attitude info // Perform IMU calculations and get attitude info
//----------------------------------------------- //-----------------------------------------------
#if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update
hil.update();
#endif
dcm.update_DCM(G_Dt); dcm.update_DCM(G_Dt);
omega = dcm.get_gyro(); omega = dcm.get_gyro();
} }

2
ArduCopterMega/Camera.pde
View File

@ -22,7 +22,7 @@ camera_stabilization()
//g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor / 2); //g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor / 2);
// dont allow control mixing // dont allow control mixing
rc_camera_pitch.servo_out = dcm.pitch_sensor / 2; g.rc_camera_pitch.servo_out = dcm.pitch_sensor / 2;
g.rc_camera_pitch.calc_pwm(); g.rc_camera_pitch.calc_pwm();
APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out); APM_RC.OutputCh(CH_5, g.rc_camera_pitch.radio_out);