Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: add dataflash logging of camera events

Browse Source
This commit is contained in:
rmackay9 2012-12-07 00:57:08 +09:00
parent d74cf54b5d
commit 3a90fc77f9
4 changed files with 61 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
ArduCopter/Log.pde
View File

@ -88,6 +88,7 @@ print_log_menu(void)
if (g.log_bitmask & MASK_LOG_PID) cliSerial->printf_P(PSTR(" PID"));
if (g.log_bitmask & MASK_LOG_ITERM) cliSerial->printf_P(PSTR(" ITERM"));
if (g.log_bitmask & MASK_LOG_INAV) cliSerial->printf_P(PSTR(" INAV"));
if (g.log_bitmask & MASK_LOG_CAMERA) cliSerial->printf_P(PSTR(" CAMERA"));
}
cliSerial->println();
@ -205,6 +206,7 @@ select_logs(uint8_t argc, const Menu::arg *argv)
TARG(PID);
TARG(ITERM);
TARG(INAV);
TARG(CAMERA);
#undef TARG
}
@ -1038,7 +1040,7 @@ static void Log_Write_DMP()
#endif
}
// Read an attitude packet
// Read a DMP attitude packet
static void Log_Read_DMP()
{
int16_t temp1 = DataFlash.ReadInt();
@ -1058,6 +1060,49 @@ static void Log_Read_DMP()
(unsigned)temp6);
}
// Write a Camera packet. Total length : 26 bytes
static void Log_Write_Camera()
{
#if CAMERA == ENABLED
DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_CAMERA_MSG);
DataFlash.WriteLong(g_gps->time); // 1
DataFlash.WriteLong(current_loc.lat); // 2
DataFlash.WriteLong(current_loc.lng); // 3
DataFlash.WriteLong(current_loc.alt); // 4
DataFlash.WriteInt((int16_t)ahrs.roll_sensor); // 5
DataFlash.WriteInt((int16_t)ahrs.pitch_sensor); // 6
DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 7
DataFlash.WriteByte(END_BYTE);
#endif
}
// Read a camera packet
static void Log_Read_Camera()
{
int32_t temp1 = DataFlash.ReadLong(); // 1 time
int32_t temp2 = DataFlash.ReadLong(); // 2 lat
int32_t temp3 = DataFlash.ReadLong(); // 3 lon
float temp4 = DataFlash.ReadLong() / 100.0; // 4 altitude
int16_t temp5 = DataFlash.ReadInt(); // 5 roll in centidegrees
int16_t temp6 = DataFlash.ReadInt(); // 6 pitch in centidegrees
uint16_t temp7 = DataFlash.ReadInt(); // 7 yaw in centidegrees
// 1
cliSerial->printf_P(PSTR("CAMERA, %ld, "),(long)temp1); // 1 time
print_latlon(cliSerial, temp2); // 2 lat
cliSerial->print_P(PSTR(", "));
print_latlon(cliSerial, temp3); // 3 lon
// 4 5 6 7
cliSerial->printf_P(PSTR(", %4.4f, %d, %d, %u\n"),
temp4, // 4 altitude
(int)temp5, // 5 roll in centidegrees
(int)temp6, // 6 pitch in centidegrees
(unsigned int)temp7); // 7 yaw in centidegrees
}
// Read the DataFlash log memory
static void Log_Read(int16_t start_page, int16_t end_page)
{
@ -1193,6 +1238,10 @@ static int16_t Log_Read_Process(int16_t start_page, int16_t end_page)
case LOG_INAV_MSG:
Log_Read_INAV();
break;
case LOG_CAMERA_MSG:
Log_Read_Camera();
break;
}
break;
case 3:

6
ArduCopter/commands_logic.pde
View File

@ -920,6 +920,10 @@ static void do_nav_roi()
// do_take_picture - take a picture with the camera library
static void do_take_picture()
{
#if CAMERA == ENABLED
g.camera.trigger_pic();
Serial.print_P(PSTR("Camera!!"));
if (g.log_bitmask & MASK_LOG_CAMERA) {
Log_Write_Camera();
}
#endif
}

5
ArduCopter/config.h
View File

@ -1003,7 +1003,10 @@
# define LOG_ITERM ENABLED
#endif
#ifndef LOG_INAV
# define LOG_INAV DISABLED
# define LOG_INAV DISABLED
#endif
#ifndef LOG_CAMERA
# define LOG_CAMERA DISABLED
#endif
// calculate the default log_bitmask

15
ArduCopter/defines.h
View File

@ -307,6 +307,7 @@ enum gcs_severity {
#define LOG_ITERM_MSG 0x0F
#define LOG_DMP_MSG 0x10
#define LOG_INAV_MSG 0x11
#define LOG_CAMERA_MSG 0x12
#define LOG_INDEX_MSG 0xF0
#define MAX_NUM_LOGS 50
@ -325,19 +326,7 @@ enum gcs_severity {
#define MASK_LOG_PID (1<<12)
#define MASK_LOG_ITERM (1<<13)
#define MASK_LOG_INAV (1<<14)
#define MASK_LOG_CTUN (1<<4)
#define MASK_LOG_NTUN (1<<5)
#define MASK_LOG_MODE (1<<6)
#define MASK_LOG_RAW (1<<7)
#define MASK_LOG_CMD (1<<8)
#define MASK_LOG_CUR (1<<9)
#define MASK_LOG_MOTORS (1<<10)
#define MASK_LOG_OPTFLOW (1<<11)
#define MASK_LOG_PID (1<<12)
#define MASK_LOG_ITERM (1<<13)
#define MASK_LOG_CAMERA (1<<15)
// DATA - event logging
#define DATA_MAVLINK_FLOAT 1