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

SITL: added dataflash logging

Browse Source
This commit is contained in:
Andrew Tridgell 2014-01-03 16:01:18 +11:00
parent d0a25b53f2
commit fcd9041e62
2 changed files with 37 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/SITL/SITL.cpp
View File

@ -20,9 +20,12 @@
*/ */
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_HAL.h>
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#include <SITL.h> #include <SITL.h>
extern const AP_HAL::HAL& hal;
// table of user settable parameters // table of user settable parameters
const AP_Param::GroupInfo SITL::var_info[] PROGMEM = { const AP_Param::GroupInfo SITL::var_info[] PROGMEM = {
AP_GROUPINFO("BARO_RND", 0, SITL, baro_noise, 3), AP_GROUPINFO("BARO_RND", 0, SITL, baro_noise, 3),
@ -85,6 +88,37 @@ void SITL::simstate_send(mavlink_channel_t chan)
state.longitude*1.0e7); state.longitude*1.0e7);
} }
/* report SITL state to DataFlash */
void SITL::Log_Write_SIMSTATE(DataFlash_Class &DataFlash)
{
double p, q, r;
float yaw;
// we want the gyro values to be directly comparable to the
// raw_imu message, which is in body frame
convert_body_frame(state.rollDeg, state.pitchDeg,
state.rollRate, state.pitchRate, state.yawRate,
&p, &q, &r);
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
struct log_AHRS pkt = {
LOG_PACKET_HEADER_INIT(LOG_SIMSTATE_MSG),
time_ms : hal.scheduler->millis(),
roll : (int16_t)(state.rollDeg*100),
pitch : (int16_t)(state.pitchDeg*100),
yaw : (uint16_t)(wrap_360_cd(yaw*100)),
alt : (float)(state.altitude*1.0e-2f),
lat : (int32_t)(state.latitude*1.0e7),
lng : (int32_t)(state.longitude*1.0e7)
};
DataFlash.WriteBlock(&pkt, sizeof(pkt));
}
// convert a set of roll rates from earth frame to body frame // convert a set of roll rates from earth frame to body frame
void SITL::convert_body_frame(double rollDeg, double pitchDeg, void SITL::convert_body_frame(double rollDeg, double pitchDeg,
double rollRate, double pitchRate, double yawRate, double rollRate, double pitchRate, double yawRate,

3
libraries/SITL/SITL.h
View File

@ -7,6 +7,7 @@
#include <AP_Common.h> #include <AP_Common.h>
#include <AP_Math.h> #include <AP_Math.h>
#include <GCS_MAVLink.h> #include <GCS_MAVLink.h>
#include <DataFlash.h>
struct PACKED sitl_fdm { struct PACKED sitl_fdm {
// this is the packet sent by the simulator // this is the packet sent by the simulator
@ -79,6 +80,8 @@ public:
void simstate_send(mavlink_channel_t chan); void simstate_send(mavlink_channel_t chan);
void Log_Write_SIMSTATE(DataFlash_Class &dataflash);
// convert a set of roll rates from earth frame to body frame // convert a set of roll rates from earth frame to body frame
static void convert_body_frame(double rollDeg, double pitchDeg, static void convert_body_frame(double rollDeg, double pitchDeg,
double rollRate, double pitchRate, double yawRate, double rollRate, double pitchRate, double yawRate,