ardupilot4.2.2-定制地面站信息

1、电池数据信息发送给地面站

        应设计要求，需要把电池的电压，电量百分比，电量，温度，循环次数等信息发送到地面站并在地面站上显示出来。

        在地面站上显示数据的文件为GCS_Common.cpp，在此文件中可以发现很多给地面站发送数据的函数，找到void GCS_MAVLINK::send_battery_status(const uint8_t instance) const，在此函数中可以给地面站发送一定的电池数据，

void GCS_MAVLINK::send_battery_status(const uint8_t instance) const
{
   /*............*/
  ///  在地面站显示电池信息  ///
    mavlink_msg_battery_status_send(chan,
                                    hal.util->battery_id,// instance, // id 电池信息
                                    MAV_BATTERY_FUNCTION_UNKNOWN, // function
                                    hal.util->battery_type,//MAV_BATTERY_TYPE_UNKNOWN, // type 类型
                                    hal.util->battery_temp_inf / 0.1f,//got_temperature ? ((int16_t) (temp * 100)) : INT16_MAX, // temperature. INT16_MAX if unknown 温度
                                    cell_volts, // cell voltages //电池电压
                                    hal.util->battery_current_inf,//current,      // current in centiampere   电流
                                    consumed_mah, // total consumed current in milliampere.hour 总消耗电流（毫安小时）
                                    consumed_wh,  // consumed energy in hJ (hecto-Joules) 消耗能量（hJ）（百焦耳）
                                    hal.util->battery_capabi_percent,//percentage, // 剩余电量百分比！bttery_cyc_life,//
                                    hal.util->battery_version,//time_remaining, // time remaining, seconds 剩余时间，秒
                                    battery.get_mavlink_charge_state(instance), // battery charge state 电池充电状态
                                    cell_volts_ext, // Cell 11..14 voltages 电池11..14电压
                                    0, // battery mode 电池模式
                                    //hal.util->battery_cyc_life,//battery.capacity_remaining_pct(instance), //电池剩余容量
                                    //hal.util->battery_version,//0, // time remaining, seconds (not provided) 剩余时间，秒（未提供）
                                    battery.get_mavlink_fault_bitmask(instance));   // fault_bitmask 错误_位掩码
 /*............*/
}

将对应的电压，百分比，电流等参数替换成我们已经设定好的数据即可。

其次还有电池的循环次数在另一个函数当中实现。

void GCS_MAVLINK::send_sys_status()
{
   /*................*/
   mavlink_msg_sys_status_send(
        chan,
        control_sensors_present,
        control_sensors_enabled,
        control_sensors_health,
        static_cast<uint16_t>(AP::scheduler().load_average() * 1000),
#if !defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BATTERY)
        battery.gcs_voltage() * 1000,  // mV
        hal.util->battery_current_inf,//battery_current,        // in 10mA units
        hal.util->battery_cyc_life,//battery_remaining,      // in %
#else
        0,
        -1,
        -1,
#endif
        0,  // comm drops %,
        0,  // comm drops in pkts,
        errors1,
        errors2,
        0,  // errors3
        errors4); // errors4
}

2、发送雷达避障数据给地面站

        同样在GCS_Common.cpp文件中，

void GCS_MAVLINK::send_proximity()
{
#if HAL_PROXIMITY_ENABLED
    AP_Proximity *proximity = AP_Proximity::get_singleton();
    if(copter.get_avoid_port() == 0){//串口雷达
    if (proximity == nullptr) {
        return; // this is wrong, but pretend we sent data and don't requeue
    }

    // get min/max distances
    const uint16_t dist_min = (uint16_t)(proximity->distance_min() * 100.0f); // minimum distance the sensor can measure in centimeters
    const uint16_t dist_max = (uint16_t)(proximity->distance_max() * 100.0f); // maximum distance the sensor can measure in centimeters

    // send horizontal distances
    if (proximity->get_status() == AP_Proximity::Status::Good) {
        AP_Proximity::Proximity_Distance_Array dist_array;
        if (proximity->get_horizontal_distances(dist_array)) {
            for (uint8_t i = 0; i < PROXIMITY_MAX_DIRECTION; i++) {
                if (!HAVE_PAYLOAD_SPACE(chan, DISTANCE_SENSOR)) {
                    return;
                }
                if (dist_array.valid(i)) {
                    proximity_ever_valid_bitmask |= (1U << i);
                } else if (!(proximity_ever_valid_bitmask & (1U << i))) {
                    // we've never sent this distance out, so we don't
                    // need to send an invalid one.
                    continue;
                }
                mavlink_msg_distance_sensor_send(
                        chan,
                        AP_HAL::millis(),                               // time since system boot
                        dist_min,                                       // minimum distance the sensor can measure in centimeters
                        dist_max,                                       // maximum distance the sensor can measure in centimeters
                        (uint16_t)(dist_array.distance[i] * 100.0f),    // current distance reading
                        MAV_DISTANCE_SENSOR_LASER,                      // type from MAV_DISTANCE_SENSOR enum
                        PROXIMITY_SENSOR_ID_START + i,                  // onboard ID of the sensor
                        dist_array.orientation[i],                      // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
                        0,                                              // Measurement covariance in centimeters, 0 for unknown / invalid readings
                        0, 0, nullptr, 0);
            }
        }
    }

    // send upward distance
    float dist_up;
    if (proximity->get_upward_distance(dist_up)) {
        if (!HAVE_PAYLOAD_SPACE(chan, DISTANCE_SENSOR)) {
            return;
        }
        mavlink_msg_distance_sensor_send(
                chan,
                AP_HAL::millis(),                                         // time since system boot
                dist_min,                                                 // minimum distance the sensor can measure in centimeters
                dist_max,                                                 // maximum distance the sensor can measure in centimeters
                (uint16_t)(dist_up * 100.0f),                             // current distance reading
                MAV_DISTANCE_SENSOR_LASER,                                // type from MAV_DISTANCE_SENSOR enum
                PROXIMITY_SENSOR_ID_START + PROXIMITY_MAX_DIRECTION + 1,  // onboard ID of the sensor
                MAV_SENSOR_ROTATION_PITCH_90,                             // direction upwards
                0,                                                        // Measurement covariance in centimeters, 0 for unknown / invalid readings
                0, 0, nullptr, 0);
    }
    }
    else if(copter.get_avoid_port() == 1){//CAN口雷达

            const uint16_t dist_min = (uint16_t)(proximity->distance_min() * 100.0f); // minimum distance the sensor can measure in centimeters
            const uint16_t dist_max = (uint16_t)(proximity->distance_max() * 100.0f); // maximum distance the sensor can measure in centimeters

            for (uint8_t i = 0; i < 8; i++)
            {
                uint32_t now = AP_HAL::millis();
                if (now - hal.util->mr72_time > 300)
                {
                    hal.util->mr72_dist[i] = 0;
                }

                mavlink_msg_distance_sensor_send(
                    chan,
                    AP_HAL::millis(),                        // time since system boot TODO: take time of measurement
                    0,               // minimum distance the sensor can measure in centimeters
                    8000,               // maximum distance the sensor can measure in centimeters
                    hal.util->mr72_dist[i],                   // current distance reading
                    MAV_DISTANCE_SENSOR_LASER,  // type from MAV_DISTANCE_SENSOR enum
                    PROXIMITY_SENSOR_ID_START + i,                                // onboard ID of the sensor == instance
                    i,                   // direction the sensor faces from MAV_SENSOR_ORIENTATION enum
                    0,                                       // Measurement covariance in centimeters, 0 for unknown / invalid readings
                    0,                                       // horizontal FOV
                    0,                                       // vertical FOV
                    (const float *)nullptr,0);                 // quaternion of sensor orientation for MAV_SENSOR_ROTATION_CUSTOM
            }

            // send upward distance
            float dist_up;
            if (proximity->get_upward_distance(dist_up)) {
                if (!HAVE_PAYLOAD_SPACE(chan, DISTANCE_SENSOR)) {
                    return;
                }
                mavlink_msg_distance_sensor_send(
                        chan,
                        AP_HAL::millis(),                                         // time since system boot
                        dist_min,                                                 // minimum distance the sensor can measure in centimeters
                        dist_max,                                                 // maximum distance the sensor can measure in centimeters
                        (uint16_t)(dist_up * 100.0f),                             // current distance reading
                        MAV_DISTANCE_SENSOR_LASER,                                // type from MAV_DISTANCE_SENSOR enum
                        PROXIMITY_SENSOR_ID_START + PROXIMITY_MAX_DIRECTION + 1,  // onboard ID of the sensor
                        MAV_SENSOR_ROTATION_PITCH_90,                             // direction upwards
                        0,                                                        // Measurement covariance in centimeters, 0 for unknown / invalid readings
                        0, 0, nullptr,0);
            }
        }
#endif // HAL_PROXIMITY_ENABLED
}
            

正确通电后，在地面站上按Ctrl+F 打开proximity即可看到雷达数据