ArduPilotは「組み込みソフト」、だったら保守性や品質をあげていきたい。

dronecodeのコーディングルールを拝見いたしました。

あまりにもざっくりしていることもあり、保守性や品質まではいき渡っていなさそうと感じています。

そこで、日本には、組み込み系ソフトについて、コーディングルールというのを事細かく定義してるものがあります。

例えば、

・MISRAとか

・IPAの「組込みソフトウェア開発向けコーディング作法ガイド[C++言語版]」や

・凡ミスによる障害の回避ルール

これらのルールをベースにArduCopterのコードの品質を上げていきたい。

また、その過程で、不安なコードがあれば、不安を軽減していきたい。

その情報や修正案などのアップしたいと思い、このディスカッションを立ち上げました。

何卒、よろしくおねがいいたします。

Views: 3277

Replies to This Discussion

ArduCopter.cpploop() メソッド

 fast_loop()メソッドが2500usを超えると以下の「uint32_t time_available」の値が不正になります。

さらに、その直後のscheduler.run()の引数の型がなんとuint16_tです。

    // run all the tasks that are due to run. Note that we only
    // have to call this once per loop, as the tasks are scheduled
    // in multiples of the main loop tick. So if they don't run on
    // the first call to the scheduler they won't run on a later
    // call until scheduler.tick() is called again
    uint32_t time_available = (timer + MAIN_LOOP_MICROS) - micros(); 
    scheduler.run(time_available);

改善案:

    uint32_t now = micros(); 

    uint32_t time_available = ((timer + MAIN_LOOP_MICROS)  < now) ? 0 : (timer + MAIN_LOOP_MICROS) - now ; 

画像は、もし、fast_loop()メソッドが2500usを超える場合を想定してdelay()メソッドにて2msしています。

実際取り込んでみました。

「no lnk」の文字が。

本来、連続2500usを超え続けると、「no link」が検出されたので効果ありと思われます。

AP_Scheduler.cpprun() メソッドについて

引数が、uint16_tですが、内部処理では、uint32_t変数と比較しています。
ということは、引数の型が不一致ということです。

検索したところ、他もuint32_tとして使用しています。

改善案:
引数の型をuint16_tをuint32_tに改善します。

===

調査内容:

/*
  run one tick
  this will run as many scheduler tasks as we can in the specified time
 */
void AP_Scheduler::run(uint16_t time_available)
{

            if (_task_time_allowed <= time_available) {

                if (time_taken >= time_available) {
                    goto update_spare_ticks;
                }
                time_available -= time_taken;


類似

Tracker、勘違いしていますね。
void Tracker::loop()
{

    scheduler.run(19900UL); uint32_tと思っているような。
}


void Rover::loop()
{

    uint32_t remaining = (timer + 20000) - micros();
    if (remaining > 19500) {
        remaining = 19500;
    }
    scheduler.run(remaining);
}

大問題!!まともに飛んでいるんだろうか?
void Plane::loop()
{
    uint32_t loop_us = 1000000UL / scheduler.get_loop_rate_hz();


    scheduler.run(loop_us);
}

Ardupilot のビルド環境に Eclipse を使用する方法があります。

その中に、librariesinclude ファイルを参照する設定がありません。

この設定をすることで、Eclipseの引用機能にてメソッドの仕様を容易に知ることができるようになります。

ぜひ、設定されることをお勧めいたします。

control_drift.cppdrift_run() メソッドについて

現状:

target_roll変数は、float変数にもかかわらず、constrain_int16を使用しています。

    // Roll velocity is feed into roll acceleration to minimize slip
    target_roll = roll_vel_error * -DRIFT_SPEEDGAIN;
    target_roll = constrain_int16(target_roll, -4500, 4500);

他については、constrain_floatを使用しております。

この変数、あるリビジョンでint16からfloatに変更になりました。

しかし、この処理については変更していませんでした。

これは、変更漏れと思われます。

改善案:

    target_roll = constrain_float(target_roll, -4500.0f, 4500.0f);

commands_logic.cppdo_nav_wp() メソッドについて(同様の処理をしているメソッド複数あります)

現状:

緯度経度「0」度の時、カレントの緯度経度を設定する様に設計されています。

存在する緯度経度「0」度をカレントの設定に使用するのはその場所でドローンを飛行させると間違った判断がされるということになります。

日本では問題になりませんが、世界をターゲットにした場合、緯度経度「0」度について意識しておく必要があるかと思います。

    if (target_loc.lat == 0 && target_loc.lng == 0) {
        target_loc.lat = current_loc.lat;
        target_loc.lng = current_loc.lng;
    }

改善案:

存在しない、緯度経度、例えば「0x80000000」にすれば良いのではと思います。

AP_Scheduler.cpprun() メソッドで各タスクの処理時間を計測について

タスクが所要時間を超えたかどうかを下記にて判定し、デバッグ情報を吐き出しています。

所要時間内に終えたタスクについて、詳細を得る仕組みが無いようです。

また、同レベルのタスクで、所要時間オーバになっても、次回も同じ番目になるため、継続して所要時間オーバになりますと、その後のタスクへの悪影響度が増す現象になります。

改善案:

・パフォーマンス測定には、各タスクの時間を計測することで、どのタスクをチューニングした方が良いかなどの判定に使えるため、ログ情報として吐き出す仕組みを追加したいと思います。

・所要時間を固定では無く、前回と今回で平均を取り、その時間をタスクテーブルに再設定することで、同レベルタスクへの悪影響度を軽減できるのではと思います。

 平均値で良いのか、最大値で良いのかは、もう少し検討が必要と思われます。

                if (time_taken > _task_time_allowed) {

:

                }

その他:「ArduCopterの上昇下降について」オール配信へのコメント

通信プロトコルとしては、「MAVproxy」を使用して行うとよいでしょう。

例えば、下記のような構築になるかと。

http://air.pawana.jp/2015/07/raspberry-pi-navio-setup/

WINに、Dronekit環境を構築して、Pythonにて開発。

http://python.dronekit.io/contributing/developer_setup_windows.html

参考例は、exampleフォルダのsImple_goto.pyをみていただければ、takeoff、移動、ランディングの処理が書かれています。

http://python.dronekit.io/examples/simple_goto.html

pixhawkもapmの流れでタスクをシーケンスに処理するように作られています。
従って、追従するタスクは前タスクの影響をもろに受けることになります。
rt、tsの場合、割り当てられた時間内に処理を終えなければ、処理が中断され、再度、時間の割当を行われます。
どうも、このような処理を中断するような仕組みを持っていないようにAP_scheduler.cppを読むと読み取れます。

アクティブに、タスクテーブルの所要時間を変更するように変更したところ、「no link」が発生しやすくなりました。また、2500us内に処理できるタスク数が多くなると全くGCSと接続できなくなりました。

処理を中断させる処理がないため、タスク処理にて姿勢制御のセンサー情報を取得できないことありそうです。

control_drift.cppdrift_init() メソッドについては、いろいろな書き方があるかも。

3項演算子って、YES or NO のような場合は、読みやすいと思うんだけど。

// drift_init - initialise drift controller
bool Copter::drift_init(bool ignore_checks)
{
    return (position_ok() || ignore_checks) ? true : false;
//    if (position_ok() || ignore_checks) {
//        return true;
//    }else{
//        return false;
//    }
}

ログ解析ツールMission Plannerで同センサーが複数ある場合の重ね合わせ表示で色を変えて、#1 #2を2回描画しているようです。

解析に即影響があるレベルではないので、後でソースを確認してみます。

最近、+/-4500の整数から、+/-1.0fのfloatに変更になった変数があったので、その関連かなと思いましたが、これについては、変更漏れというより、こちらのコミットで始めからconstrain_int16()として追加されたもののようです。


https://github.com/ArduPilot/ardupilot/commit/fe74a11b4ee8b310ec102...

他にも、明示的に型変換をしていない箇所は山ほどあります。

https://github.com/ArduPilot/ardupilot/issues/4493

上記ポストによると、変数の型を変更する際に気をつけることは、以下の2点だそうです。

1) Linuxでの挙動が変わらないこと

2) PX4のフラッシュサイズが増加しないこと(Pixhawk(PX4FUMv2)ではすでにフラッシュ容量が足りなくなり、不要なモジュールの削除が必要になってきています。Pixracerはまだ余裕があります)

せっかくここまでお調べになって、変更案もあるのですから、GitHubでissueを作るか、pull requestを投げてみてはいかがでしょう。

murata,katsutoshi said:

control_drift.cppdrift_run() メソッドについて

現状:

target_roll変数は、float変数にもかかわらず、constrain_int16を使用しています。

    // Roll velocity is feed into roll acceleration to minimize slip
    target_roll = roll_vel_error * -DRIFT_SPEEDGAIN;
    target_roll = constrain_int16(target_roll, -4500, 4500);

他については、constrain_floatを使用しております。

この変数、あるリビジョンでint16からfloatに変更になりました。

しかし、この処理については変更していませんでした。

これは、変更漏れと思われます。

改善案:

    target_roll = constrain_float(target_roll, -4500.0f, 4500.0f);

RSS

© 2017   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service