void LR_MsgHandler_IMT_Base::update_from_msg_imt(uint8_t imu_offset, uint8_t *msg)
{
    wait_timestamp_from_msg(msg);

    if (!use_imt) {
        return;
    }

    uint8_t this_imu_mask = 1 << imu_offset;

    float delta_time = 0;
    require_field(msg, "DelT", delta_time);
    ins.set_delta_time(delta_time);

    if (gyro_mask & this_imu_mask) {
        Vector3f d_angle;
        require_field(msg, "DelA", d_angle);
        float d_angle_dt;
        if (!field_value(msg, "DelaT", d_angle_dt)) {
            d_angle_dt = 0;
        }
        ins.set_delta_angle(imu_offset, d_angle, d_angle_dt);
    }
    if (accel_mask & this_imu_mask) {
        float dvt = 0;
        require_field(msg, "DelvT", dvt);
        Vector3f d_velocity;
        require_field(msg, "DelV", d_velocity);
        ins.set_delta_velocity(imu_offset, dvt, d_velocity);
    }
}
void LR_MsgHandler_CHEK::process_message(uint8_t *msg)
{
    wait_timestamp_from_msg(msg);
    check_state.time_us = AP_HAL::micros64();
    attitude_from_msg(msg, check_state.euler, "Roll", "Pitch", "Yaw");
    check_state.euler *= radians(1);
    location_from_msg(msg, check_state.pos, "Lat", "Lng", "Alt");
    require_field(msg, "VN", check_state.velocity.x);
    require_field(msg, "VE", check_state.velocity.y);
    require_field(msg, "VD", check_state.velocity.z);
}
Exemple #3
0
void MsgHandler::ground_vel_from_msg(uint8_t *msg,
                                     Vector3f &vel,
                                     const char *label_speed,
                                     const char *label_course,
                                     const char *label_vz)
{
    uint32_t ground_speed;
    int32_t ground_course;
    require_field(msg, label_speed, ground_speed);
    require_field(msg, label_course, ground_course);
    vel[0] = ground_speed*0.01f*cosf(radians(ground_course*0.01f));
    vel[1] = ground_speed*0.01f*sinf(radians(ground_course*0.01f));
    vel[2] = require_field_float(msg, label_vz);
}
void LR_MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
{
    wait_timestamp_from_msg(msg);

    Vector3f mag;
    require_field(msg, "Mag", mag);
    Vector3f mag_offset;
    require_field(msg, "Ofs", mag_offset);

    compass.setHIL(compass_offset, mag - mag_offset);
    // compass_offset is which compass we are setting info for;
    // mag_offset is a vector indicating the compass' calibration...
    compass.set_offsets(compass_offset, mag_offset);
}
void LR_MsgHandler_PARM::process_message(uint8_t *msg)
{
    const uint8_t parameter_name_len = AP_MAX_NAME_SIZE + 1; // null-term
    char parameter_name[parameter_name_len];
    uint64_t time_us;

    if (field_value(msg, "TimeUS", time_us)) {
        wait_timestamp_usec(time_us);
    } else {
        // older logs can have a lot of FMT and PARM messages up the
        // front which don't have timestamps.  Since in Replay we run
        // DataFlash's IO only when stop_clock is called, we can
        // overflow DataFlash's ringbuffer.  This should force us to
        // do IO:
        hal.scheduler->stop_clock(last_timestamp_usec);
    }

    require_field(msg, "Name", parameter_name, parameter_name_len);

    float value = require_field_float(msg, "Value");
    if (globals.no_params) {
        printf("Not changing %s to %f\n", parameter_name, value);
    } else {
        set_parameter(parameter_name, value);
    }
}
void LR_MsgHandler_IMU_Base::update_from_msg_imu(uint8_t imu_offset, uint8_t *msg)
{
    wait_timestamp_from_msg(msg);

    uint8_t this_imu_mask = 1 << imu_offset;

    if (gyro_mask & this_imu_mask) {
        Vector3f gyro;
        require_field(msg, "Gyr", gyro);
        ins.set_gyro(imu_offset, gyro);
    }
    if (accel_mask & this_imu_mask) {
        Vector3f accel2;
        require_field(msg, "Acc", accel2);
        ins.set_accel(imu_offset, accel2);
    }
}
void LR_MsgHandler_MAG_Base::update_from_msg_compass(uint8_t compass_offset, uint8_t *msg)
{
    wait_timestamp_from_msg(msg);

    Vector3f mag;
    require_field(msg, "Mag", mag);
    Vector3f mag_offset;
    require_field(msg, "Ofs", mag_offset);
    uint32_t last_update_usec;
    if (!field_value(msg, "S", last_update_usec)) {
        last_update_usec = AP_HAL::micros();
    }

    compass.setHIL(compass_offset, mag - mag_offset, last_update_usec);
    // compass_offset is which compass we are setting info for;
    // mag_offset is a vector indicating the compass' calibration...
    compass.set_offsets(compass_offset, mag_offset);
}
void LR_MsgHandler_GPA_Base::update_from_msg_gpa(uint8_t gps_offset, uint8_t *msg)
{
    uint64_t time_us;
    require_field(msg, "TimeUS", time_us);
    wait_timestamp_usec(time_us);

    uint16_t vdop, hacc, vacc, sacc;
    require_field(msg, "VDop", vdop);
    require_field(msg, "HAcc", hacc);
    require_field(msg, "VAcc", vacc);
    require_field(msg, "SAcc", sacc);
    uint8_t have_vertical_velocity;
    if (! field_value(msg, "VV", have_vertical_velocity)) {
        have_vertical_velocity = !is_zero(gps.velocity(gps_offset).z);
    }
    uint32_t sample_ms;
    if (! field_value(msg, "SMS", sample_ms)) {
        sample_ms = 0;
    }

    gps.setHIL_Accuracy(gps_offset, vdop*0.01f, hacc*0.01f, vacc*0.01f, sacc*0.01f, have_vertical_velocity, sample_ms);
}
void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg, bool responsible_for_relalt)
{
    uint64_t time_us;
    if (! field_value(msg, "TimeUS", time_us)) {
        uint32_t timestamp;
        require_field(msg, "T", timestamp);
        time_us = timestamp * 1000;
    }
    wait_timestamp_usec(time_us);

    Location loc;
    location_from_msg(msg, loc, "Lat", "Lng", "Alt");
    Vector3f vel;
    ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");

    uint8_t status = require_field_uint8_t(msg, "Status");
    uint8_t hdop = 0;
    if (! field_value(msg, "HDop", hdop) &&
        ! field_value(msg, "HDp", hdop)) {
        hdop = 20;
    }
    uint8_t nsats = 0;
    if (! field_value(msg, "NSats", nsats) &&
        ! field_value(msg, "numSV", nsats)) {
        field_not_found(msg, "NSats");
    }
    gps.setHIL(gps_offset,
               (AP_GPS::GPS_Status)status,
               uint32_t(time_us/1000),
               loc,
               vel,
               nsats,
               hdop,
               require_field_float(msg, "VZ") != 0);
    if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
        ground_alt_cm = require_field_int32_t(msg, "Alt");
    }

    if (responsible_for_relalt) {
        // this could possibly check for the presence of "RelAlt" label?
        int32_t tmp;
        if (! field_value(msg, "RAlt", tmp)) {
            tmp = require_field_int32_t(msg, "RelAlt");
        }
        rel_altitude = 0.01f * tmp;
    }
}
Exemple #10
0
void LR_MsgHandler_GPS_Base::update_from_msg_gps(uint8_t gps_offset, uint8_t *msg)
{
    uint64_t time_us;
    if (! field_value(msg, "TimeUS", time_us)) {
        uint32_t timestamp;
        require_field(msg, "T", timestamp);
        time_us = timestamp * 1000;
    }
    wait_timestamp_usec(time_us);

    Location loc;
    location_from_msg(msg, loc, "Lat", "Lng", "Alt");
    Vector3f vel;
    ground_vel_from_msg(msg, vel, "Spd", "GCrs", "VZ");

    uint8_t status = require_field_uint8_t(msg, "Status");
    uint8_t hdop = 0;
    if (! field_value(msg, "HDop", hdop) &&
        ! field_value(msg, "HDp", hdop)) {
        hdop = 20;
    }
    uint8_t nsats = 0;
    if (! field_value(msg, "NSats", nsats) &&
        ! field_value(msg, "numSV", nsats)) {
        field_not_found(msg, "NSats");
    }
    uint16_t GWk;
    uint32_t GMS;
    if (! field_value(msg, "GWk", GWk)) {
        field_not_found(msg, "GWk");
    }
    if (! field_value(msg, "GMS", GMS)) {
        field_not_found(msg, "GMS");
    }
    gps.setHIL(gps_offset,
               (AP_GPS::GPS_Status)status,
               AP_GPS::time_epoch_convert(GWk, GMS),
               loc,
               vel,
               nsats,
               hdop);
    if (status == AP_GPS::GPS_OK_FIX_3D && ground_alt_cm == 0) {
        ground_alt_cm = require_field_int32_t(msg, "Alt");
    }
}
Exemple #11
0
void LR_MsgHandler_MSG::process_message(uint8_t *msg)
{
    const uint8_t msg_text_len = 64;
    char msg_text[msg_text_len];
    require_field(msg, "Message", msg_text, msg_text_len);

    if (strncmp(msg_text, "ArduPlane", strlen("ArduPlane")) == 0) {
	vehicle = VehicleType::VEHICLE_PLANE;
	::printf("Detected Plane\n");
	ahrs.set_vehicle_class(AHRS_VEHICLE_FIXED_WING);
	ahrs.set_fly_forward(true);
    } else if (strncmp(msg_text, "ArduCopter", strlen("ArduCopter")) == 0 ||
	       strncmp(msg_text, "APM:Copter", strlen("APM:Copter")) == 0) {
	vehicle = VehicleType::VEHICLE_COPTER;
	::printf("Detected Copter\n");
	ahrs.set_vehicle_class(AHRS_VEHICLE_COPTER);
	ahrs.set_fly_forward(false);
    } else if (strncmp(msg_text, "ArduRover", strlen("ArduRover")) == 0) {
	vehicle = VehicleType::VEHICLE_ROVER;
	::printf("Detected Rover\n");
	ahrs.set_vehicle_class(AHRS_VEHICLE_GROUND);
	ahrs.set_fly_forward(true);
    }
}
Exemple #12
0
int16_t MsgHandler::require_field_int16_t(uint8_t *msg, const char *label)
{
    int16_t ret;
    require_field(msg, label, ret);
    return ret;
}
Exemple #13
0
float MsgHandler::require_field_float(uint8_t *msg, const char *label)
{
    float ret;
    require_field(msg, label, ret);
    return ret;
}
Exemple #14
0
void MsgHandler::wait_timestamp_from_msg(uint8_t *msg)
{
    uint32_t timestamp;
    require_field(msg, "TimeMS", timestamp);
    wait_timestamp(timestamp);
}